hisi3516上调试mpu6500

一、平台:hisi3516  

         imu模块:mpu6500   采集方式：spi 传输到pc：socket

开发板上源码：

 #include

#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include




//#define SINGLE_READ 


#define SERVER_PORT    8888 
#define LENGTH_OF_LISTEN_QUEUE 20
#define BUFFER_SIZE 1024




static const char *device = "/dev/spidev0.0";
static unsigned char mode = SPI_MODE_3  ; 
static unsigned char bits = 16; 
static uint32_t speed = 12*1000*1000;/* 设置spi传输速度 */




static int g_fd = -1;


struct mpu6500_data {
   int16_t  accel_raw[3];
   int16_t  gyro_raw[3];
   int16_t  temprature;
   double   timeStampe;


   unsigned int index;
};




double gettimestamp()
{
struct timeval tv;
gettimeofday(&tv, NULL);
return tv.tv_usec + tv.tv_sec*1e6;
}


int SPI_Fast_Transfer(unsigned char *tx , unsigned char *rx, unsigned int len)
{

int ret = 0;
    struct spi_ioc_transfer mesg[1];

    memset(mesg, 0, sizeof(mesg));
    mesg[0].tx_buf = (__u64)tx;
    mesg[0].len    = (__u32)len;
    mesg[0].rx_buf = (__u64)rx;
    mesg[0].cs_change = 1;




    ret = ioctl(g_fd, SPI_IOC_MESSAGE(1), mesg);
    if (ret  < 0) {  
        printf("SPI_IOC_MESSAGE error \n");  
        return -1;  
    }
}




int sensor_write_register(unsigned char addr, unsigned char data)
{
    int ret;
    struct spi_ioc_transfer mesg[1];
    unsigned char  tx_buf[8] = {0};
    unsigned char  rx_buf[8] = {0};
    
    tx_buf[0] = data;
    tx_buf[1] = addr & (~0x80);




    memset(mesg, 0, sizeof(mesg));  
    mesg[0].tx_buf = (__u64)tx_buf;  
    mesg[0].len    = 2;  
    mesg[0].rx_buf = (__u64)rx_buf; 
    mesg[0].cs_change = 1;


    ret = ioctl(g_fd, SPI_IOC_MESSAGE(1), mesg);
    if (ret < 0) {  
        printf("SPI_IOC_MESSAGE error \n");  
        return -1;  
    }
 
    return 0;
}


int sensor_read_register(unsigned char addr)
{
    int ret = 0;
    struct spi_ioc_transfer mesg[1];
    unsigned char  tx_buf[8] = {0};
    unsigned char  rx_buf[8] = {0};
    
tx_buf[0] = 0;
    tx_buf[1] = addr | 0x80;


    
    memset(mesg, 0, sizeof(mesg));
    mesg[0].tx_buf = (__u64)tx_buf;
    mesg[0].len    = 2;
    mesg[0].rx_buf = (__u64)rx_buf;
    mesg[0].cs_change = 1;


    ret = ioctl(g_fd, SPI_IOC_MESSAGE(1), mesg);
    if (ret  < 0) {  
        printf("SPI_IOC_MESSAGE error \n");  
        return -1;  
    }
    
    return rx_buf[0];
}




/**
*设置imu寄存器
**/


int set_imu_register(void)
{
sensor_write_register(0x56, 0x00);
usleep(1000);
sensor_write_register(0x1A,0x07);  //low pass filter   [2:0]
    usleep(1000);
    sensor_write_register(0x1B,0x1B);  //GYRO_CONFIG   XG_ST  YG_ST  ZG_ST  FS_SEL        Resv   FCHOICE_B   //00为启用
    usleep(1000);                  
    sensor_write_register(0x1C,0x18);  //ACCEL_CONFIG  XA_ST  YA_ST  ZA_ST  FS_SEL        Resv
    usleep(1000);                       
    sensor_write_register(0x1D,0x08);  //ACCEL_CONFIG2 Resv                        FCHOICE_B     A_DLPF_CFG
usleep(1000);
sensor_write_register(0x6B, 0x00);
usleep(1000);
    sensor_write_register(0x19,0x00);  
usleep(1000);
    
return 0;
}


/**
* 功 能：打开设备 并初始化设备
* 入口参数 ：
* 出口参数：
* 返回值：0 表示已打开 
*/
int spi_sensor_open(void)
{
int fd;
int ret = 0;


fd = open(device, O_RDWR);
if (fd < 0)
printf("can't open device\n");
else
printf("SPI - Open Succeed. Start Init SPI...\n");




g_fd = fd;
/*
* spi mode
*/
ret = ioctl(fd, SPI_IOC_WR_MODE, &mode);
if (ret == -1)
printf("can't set spi mode\n");




ret = ioctl(fd, SPI_IOC_RD_MODE, &mode);
if (ret == -1)
printf("can't get spi mode\n");




/*
* bits per word
*/
ret = ioctl(fd, SPI_IOC_WR_BITS_PER_WORD, &bits);
if (ret == -1)
printf("can't set bits per word\n");




ret = ioctl(fd, SPI_IOC_RD_BITS_PER_WORD, &bits);
if (ret == -1)
printf("can't get bits per word\n");




/*
* max speed hz
*/
ret = ioctl(fd, SPI_IOC_WR_MAX_SPEED_HZ, &speed);
if (ret == -1)
printf("can't set max speed hz\n");




ret = ioctl(fd, SPI_IOC_RD_MAX_SPEED_HZ, &speed);
if (ret == -1)
printf("can't get max speed hz\n");




printf("spi mode: %d\n", mode);
printf("bits per word: %d\n", bits);
printf("max speed: %d KHz (%d MHz)\n", speed / 1000, speed / 1000 / 1000);




return ret;
}




/**
* 功 能：关闭SPI模块
*/
int spi_senor_close(void)
{
int fd = g_fd;




if (fd == 0) /* SPI是否已经打开*/
return 0;
close(fd);
g_fd = 0;


return 0;
}




static unsigned int imu_index ;
int mpu6500_get_data(struct mpu6500_data *data)
{

#ifdef SINGLE_READ
data->accel_raw[0] = sensor_read_register(0x3b) << 0x08;
data->accel_raw[0] |= sensor_read_register(0x3c);


data->accel_raw[1] = sensor_read_register(0x3d) << 0x08;
data->accel_raw[1] |= sensor_read_register(0x3e);

data->accel_raw[2] = sensor_read_register(0x3f) << 0x08;
data->accel_raw[2] |= sensor_read_register(0x40);

data->temprature = sensor_read_register(0x41) << 0x08;
data->temprature |= sensor_read_register(0x42);

data->gyro_raw[0] = sensor_read_register(0x43) << 0x08;
data->gyro_raw[0] |= sensor_read_register(0x44);

data->gyro_raw[1] = sensor_read_register(0x45) << 0x08;
data->gyro_raw[1] |= sensor_read_register(0x46);

data->gyro_raw[2] = sensor_read_register(0x47) << 0x08;
data->gyro_raw[2] |= sensor_read_register(0x48);

#else

unsigned char tx_buf[14] = {0};
unsigned char rx_buf[14] = {0};
tx_buf[1] = 0x3b | 0x80;
SPI_Fast_Transfer(tx_buf , rx_buf, 14);

data->accel_raw[0] = rx_buf[0] << 0x08;
data->accel_raw[0] |= rx_buf[1];


data->accel_raw[1] = rx_buf[2] << 0x08;
data->accel_raw[1] |= rx_buf[3];

data->accel_raw[2] = rx_buf[4] << 0x08;
data->accel_raw[2] |= rx_buf[5];

data->temprature = rx_buf[6] << 0x08;
data->temprature |= rx_buf[7];

data->gyro_raw[0] = rx_buf[8] << 0x08;
data->gyro_raw[0] |= rx_buf[9];

data->gyro_raw[1] = rx_buf[10] << 0x08;
data->gyro_raw[1] |= rx_buf[11];

data->gyro_raw[2] = rx_buf[12] << 0x08;
data->gyro_raw[2] |= rx_buf[13];
#endif


data->timeStampe = gettimestamp();

imu_index ++;

data->index = imu_index;


return 0;
}




static unsigned int i ; 
static double time_last  , time_cur ;
static float error_rate;
static unsigned int lost_num;
void show_imu(struct mpu6500_data temp)
{
time_last = time_cur;
time_cur = temp.timeStampe; 
if(abs(time_cur - time_last) > 200)
{
i++;
}
lost_num = i;
error_rate = (float)i/(float)imu_index;

printf("accel_x = %d, accel_y = %d, accel_z = %d\n", temp.accel_raw[0], temp.accel_raw[1], temp.accel_raw[2]);
printf("gyro_x = %d,  gyro_y = %d,  gyro_z = %d\n", temp.gyro_raw[0], temp.gyro_raw[1], temp.gyro_raw[2]);
printf("temprature = %d, timeStampe = %f , lost_data = %d, lost_data_rate = %f, index = %d\n", temp.temprature, temp.timeStampe, i, error_rate, imu_index );
}






 
int main(int argc, char **argv)
{

    struct sockaddr_in server_addr;
    bzero(&server_addr,sizeof(server_addr)); 
    server_addr.sin_family = AF_INET;
    server_addr.sin_addr.s_addr = htons(INADDR_ANY);
    server_addr.sin_port = htons(SERVER_PORT);



 
    //创建用于internet的流协议(TCP)socket,用server_socket代表服务器socket
    int server_socket = socket(PF_INET,SOCK_STREAM,0);
    if( server_socket < 0)
    {
        printf("Create Socket Failed!");
        exit(1);
    }
else
{
int opt =1;
setsockopt(server_socket,SOL_SOCKET,SO_REUSEADDR,&opt,sizeof(opt));
}



     
   
    if( bind(server_socket,(struct sockaddr*)&server_addr,sizeof(server_addr)))
    {
        printf("Server Bind Port : %d Failed!", SERVER_PORT); 
        exit(1);
    }
 


    if ( listen(server_socket, LENGTH_OF_LISTEN_QUEUE) )
    {
        printf("Server Listen Failed!"); 
        exit(1);
    }



/*设置imu*/
spi_sensor_open();
set_imu_register();


struct mpu6500_data data; 
    while (1) 
    {
        //定义客户端的socket地址结构client_addr
        struct sockaddr_in client_addr;
        socklen_t length = sizeof(client_addr);

        int new_fd = accept(server_socket,(struct sockaddr*)&client_addr,&length);
        if ( new_fd < 0)
        {
            printf("Server Accept Failed!\n");
            break;
        }
else{
printf("server: got connection from %s\n",inet_ntoa(client_addr.sin_addr));  
}

        while(1)  
        {  

            mpu6500_get_data(&data); 
            show_imu(data);


length = send(new_fd, &data, sizeof(struct mpu6500_data), 0);  
            if (length < 0)  
            {  
                printf("Server send Data Failed!\n");  
                break;  
            }

        }     
        
    }

close(server_socket);


while(waitpid(-1,NULL,WNOHANG) > 0); 
    return 0;
}

window上源码：

#include "StdAfx.h"
#include
#include
#include
#include
#pragma comment(lib,"ws2_32.lib")
#include
#define FILE_PATH  "imu_data.txt"




#define PORT 8888


using namespace std;


struct mpu6500_data 
{
   __int16  accel_raw[3];
   __int16  gyro_raw[3];
   __int16  temprature;
   double   timeStampe;
   unsigned int index;
};




int main(int argc, char **args )
{


char SendBuffer[1024];
FILE *f ;
fopen_s(&f, FILE_PATH , "wb");
if(f < 0){
printf("FILE OPEN FAILED\n");
}




    WSADATA wsa;
    //初始化套接字DLL
    if(WSAStartup(MAKEWORD(2,2),&wsa)!=0)
{
        printf("套接字初始化失败!");
        exit(-1);
    }
    //创建套接字
    int sock;
    if((sock=socket(AF_INET,SOCK_STREAM,IPPROTO_TCP))==INVALID_SOCKET)
{
        printf("创建套接字失败！");
        exit(-1);
    }


    struct sockaddr_in serverAddress;
    memset(&serverAddress,0,sizeof(sockaddr_in));
    serverAddress.sin_family=AF_INET;
    serverAddress.sin_addr.S_un.S_addr = inet_addr("192.168.7.101");
    serverAddress.sin_port = htons(PORT);
    //建立和服务器的连接
    if(connect(sock,(sockaddr*)&serverAddress,sizeof(serverAddress))==SOCKET_ERROR)
{
        printf("connet failed ！");
        exit(-1);
    }
else
{
printf("connect ok !\n"); 

}


char RecvBuffer[4096] = {0} ;
char WriteBuffer[4096] = {0};
struct mpu6500_data *data = (struct mpu6500_data * )malloc(sizeof(struct mpu6500_data));


static unsigned int lost_num;
static unsigned int last_index, cur_index;
while (1)  
    {  
  
        //recv data from server  
int Ret = recv(sock, RecvBuffer, sizeof(struct mpu6500_data), 0);  
        if ( Ret == SOCKET_ERROR )  
        {  
           printf("recv Info Error::%d\n", GetLastError());  
           break;  
        }
else
{  
data = (struct mpu6500_data *)RecvBuffer;
last_index = cur_index;
cur_index = data->index;
if(cur_index - last_index > 1)
{
lost_num ++;
}


fprintf(f, "%d  %d  %d\n", data->accel_raw[0], data->accel_raw[1], data->accel_raw[2]);
fprintf(f, "%d  %d  %d\n", data->gyro_raw[0],  data->gyro_raw[1],  data->gyro_raw[2]); 
fprintf(f, "%d  %f  %d  %d\n", data->temprature, data->timeStampe , data->index, lost_num);

fflush(f);
}
     
}  

fclose(f);


    WSACleanup();

    return 0;
}

*************************************************************************************

主要调试时间消耗在spi通信上，由于之前对spi通信不太熟悉，mpu6500 spi通信是采用16bit通信， 采用突发模式读取数据可以实现快速读取数据。    mpu6500最快输出频率为8000hz。