GPS定位模块惯导功能的软件实现     DATE: 2020-03-19 19:13

【ADR】惯性导航模块
 
GPS定位模块惯导功能的软件实现
 
【实现步骤】
1. 配置天线位置
接口定义如下:
int set_antenna_lever_arm(int16_t x, int16_t y, int16_t z)
{
    if (x == COORDINATE_NON ||  y == COORDINATE_NON ||  z == COORDINATE_NON) {
        V2X_ERR("antenna lever arm paremeter is empty");
        return -1;
    }
    _set_lever_arm(&cmd_set_antenna_lever_arm, x, y, z);
    _encapsule_crc(&cmd_set_antenna_lever_arm);
 
    return gps_run_cmd_s(&cmd_set_antenna_lever_arm);
}
 
2.配置陀螺仪角度
接口定义如下:
int set_user_sensor_alignment(int16_t x, int16_t y, int16_t z)
{    
     if (x == COORDINATE_NON ||  y == COORDINATE_NON ||  z == COORDINATE_NON) {
        V2X_ERR("sensor alignment paremeter is empty");
        return -1;
    }
 
    if (x < 0 || x > 360 || y < -90 || y > 90 || z < -180 || z > 180) {
        V2X_ERR("sensor alignment paremeter is invalid. yaw(x): [0..360], pitch(y):[-90..90], roll(z):[-180..180]");
        return -1;
    }
    
    putle32(cmd_set_user_sensor_alignment.cmd, UBX_DATA_OFFSET + 4, (uint32_t)x);
    putle16(cmd_set_user_sensor_alignment.cmd, UBX_DATA_OFFSET + 8, y);
    putle16(cmd_set_user_sensor_alignment.cmd, UBX_DATA_OFFSET + 10, z);
    
    _encapsule_crc(&cmd_set_user_sensor_alignment);
 
    return gps_run_cmd_s(&cmd_set_user_sensor_alignment);
}
 
3. 从obd获取速度,作为惯导模块输入
 
obd套接字接口初始化:
int obd_socket_init()
{
    struct sockaddr_in servaddr;
 
bzero(&servaddr , sizeof(servaddr));  
servaddr.sin_family = AF_INET;  
servaddr.sin_port = htons(OBD_SOCKET_SERV_PORT);
servaddr.sin_addr.s_addr = inet_addr(OBD_SOCKET_SERV_ADDRESS); 
 
sockfd = socket(AF_INET,SOCK_STREAM , 0);
 
        if (sockfd == -1) {  
        V2X_ERR("socket error");  
    goto out; 
}
        
if (connect(sockfd, (struct sockaddr *)&servaddr , sizeof(servaddr)) < 0) {  
V2X_ERR("connect error");  
goto out;
}
 
        return 0;
    
out:
       if (sockfd) {
           close(sockfd);
       }
 
       return -1;             
}
 
接收到速度后,转发给gps芯片
 
static void* set_obd_speed_measurement_timer(void * arg)
{
    char buff[64];
    int32_t speed;
    int len;
    char *str;
    char *obd = "41 0D";
    
    while (1)
    {
try_recv:
        str = NULL;
        memset(buff, 0, sizeof(buff));
        len = recv(sockfd, buff, sizeof(buff), 0);
 
        if (len == 0) {
           
            V2X_WARN("speed measurements socket client was shutdown.");
            goto try_recv;
        } else if(len < 0) {
           
            V2X_ERR("read error from speed measurements socket client, %s", strerror(errno));
            goto try_recv;
        } else {
            V2X_DBG("obd buffer is %s\n",buff);
        
            str = search_lastplace(obd, buff);
 
            if (str == NULL) {
                V2X_ERR("no speed get from obd\n");
                goto try_recv;
            } 
 
            speed = convert(2, str + 2);
        }
 
       
        if (speed > 300 || speed < -100) {
            V2X_WARN("wrong speed measurements received from socket client. speed was invalid: %d millimeter/h", speed);
            goto try_recv;
        }
        
        speed = (int32_t)speed * 1000 * 1000 / 3600;
 
        V2X_INFO("speed is %d mm per second\n", speed);
        
        sigroutine(SIG, NULL, &speed); 
    }
 
    pthread_exit(NULL);
    
}