devbox
2023面试高手
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

ADIS16465 加速度和陀螺仪数据融合及调试（kalman滤波）

作者：2023面试高手 | 2024-06-11 15:57:46

赞

踩

adis16465

先通过SPI读取加速度和角速度，然后在单片机里通过通过kalman滤波进行数据融合，输出X轴角度，同时通过CAN口将融合前的原始数据和融合的角度发送给上位机，上位机借用基于ROS的Plotjuggler绘图工具，进行数据的图形化显示，极大的方便了滤波参数的调整。

 

下面贴出调试好的kalman滤波部分代码，以及ROS部分代码。

kalman滤波代码（借用他人代码修改了极少部分，原作者看到请留意我好署名原作者）：

其中参数acc_x,y,z为三轴加速度值，有的代码只取了x轴，考虑非理想状态，最好是三轴都取

  1. #define DEG2RAD 	0.017453292519943295769236907684886f	// 角度转弧度
  2. #define RAD2DEG 	57.295779513082320876798154814105f	    // 弧度转角度
  3. float Q_angle = 0.0008f;	    // 陀螺仪噪声的协方差
  4. float R_measure = 0.05f;		// 加速度计的协方差
  5. float Q_bias = 0.0008f;		    // Q_bias为陀螺仪漂移	
  6. float dt = 0.005f;
  7. float bias = 0;	
  8. float k_0 = 0,k_1 = 0; 					// 卡尔曼增益
  9. float pdot[4] = { 0.0f,0.0f,0.0f,0.0f };
  10. float p[2][2] = { {1.0f,0.0f},{0.0f,1.0f} };
  11.  
  12. float kalman(float acc_x,float acc_y,float acc_z,float gyro_x)
  13. {   
  14.     static float angle = 0.0f;				// 卡尔曼滤波器的输岀值，最优估计的角度 
  15.     float angle_acc,acc1_z,acc1_x,acc1_y;
  16.     acc1_z = acc_z * 0.00025;
  17.     acc1_x = acc_x * 0.00025;
  18.     acc1_y = acc_y * 0.00025;
  19.     angle_acc = atan2f(acc1_x,sqrtf(acc1_z*acc1_z + acc1_y*acc1_y)) * RAD2DEG;
  20.  
  21. 	//1、卡尔曼第一个公式(状态预测):X(k|k-1)=AX(k-1|k-1)+BU(k)
  22. 	angle  += (gyro_x/40.0 - bias) * dt;
  23. 	
  24. 	//2、卡尔曼第二个公式(计算误差协方差)：AP(k-1|k-1)A' + Q
  25. 	pdot[0] = Q_angle - p[0][1] + p[1][0];
  26. 	pdot[1] = -p[1][1];
  27. 	pdot[2] = -p[1][1];
  28. 	pdot[3] = Q_bias;
  29. 	
  30. 	p[0][0] += pdot[0] * dt;
  31. 	p[0][1] += pdot[1] * dt;
  32. 	p[1][0] += pdot[2] * dt;
  33. 	p[1][1] += pdot[3] * dt;
  34. 	
  35. 	//3、卡尔曼第三个公式(计算卡尔曼增益):Kg(k) = P(k|k-1)H' / (HP(k|k-1)H' + R)
  36. 	//R --->系统测量噪声协方差
  37. 	k_0 = p[0][0]/(p[0][0]+R_measure);
  38. 	k_1 = p[1][0]/(p[0][0]+R_measure);
  39. 	
  40. 	//4、卡尔曼第四个公式(修正估计):X(k|k) = X(k|k-1) + Kg(k)(Z(k) - HX(k|k-1))
  41. 	angle	+= k_0	* (angle_acc - angle);
  42. 	bias += k_1  * (angle_acc -	angle);
  43. 	
  44. 	//5、卡尔曼第五个公式(更新误差协方差)：P(k|k) = (1 - Kg(k)H) P(k|k-1)
  45. 	p[0][0] = (1 - k_0) * p[0][0];
  46. 	p[0][1] = (1 - k_0) * p[0][1];
  47. 	p[1][0] = (1 - k_0) * p[1][0];
  48. 	p[1][1] = (1 - k_0) * p[1][1];
  49. 	return angle;
  50. }

ROS下CAN部分代码（使用的珠海创芯科技的USB转CAN），main部分主要是进行can设备的初始化，以及初始化话题等，在主循环进行can消息的发送、IMU数据话题发布，IMU数据在后面的can主代码里通过读取can口获取。其中bms结构为发布的的IMU数据（修改了之前代码，懒得重命名，将就着看）将在后面的Plotjuggler里订阅。

  1. int main(int argc, char** argv)
  2. {
  3. 	pthread_t tid;
  4.   	ros::init(argc, argv, "abc");
  5. 	ros::NodeHandle n,nh,ns;
  6.   	std::string can_device;
  7.  
  8.     ros::Publisher bat_pub = n.advertise<std_msgs::Float32MultiArray>("bms", 1000); 
  9.  
  10. 	std_msgs::Float32MultiArray bms;
  11. 	bms.layout.dim.push_back(std_msgs::MultiArrayDimension());
  12. 	bms.layout.dim[0].size = 6;
  13. 	bms.layout.dim[0].stride = 6; 
  14. 	bms.layout.dim[0].label = "bms";
  15. 	bms.data.resize(6);
  16.  
  17.  
  18.     signal(SIGALRM, sigHandler);
  19.     signal(SIGINT, sigHandler);
  20.  
  21. 	if(initCAN(VCI_USBCAN2) == 0)		// Initialization device
  22.         exit(0);
  23. 		
  24. 	getCANInfo(VCI_USBCAN2);			// get device information	
  25. 	 
  26.     if(pthread_create(&tid,NULL,receiveFrame,(void *)&run) != 0)
  27. 		ROS_INFO("Thread: Create can1 thread failed!");
  28.  
  29.     frame = (PVCI_CAN_OBJ)malloc(sizeof(VCI_CAN_OBJ));
  30.     memset(frame,0x00,sizeof(VCI_CAN_OBJ));
  31.  
  32.     ros::Rate loop_rate(100);
  33.  
  34. 	while (ros::ok()) 
  35.     {
  36. 		frame->id = 0x361;
  37. 		frame->dlc = 4;
  38. 		frame->data[0] = 0;
  39. 		frame->data[1] = 200;
  40. 		frame->data[2] = 0;
  41. 		frame->data[3] = 0;
  42. 		frame->is_extended = false;
  43. 		frame->is_rtr = false;
  44. 		sendCTLFrame(frame);
  45.  
  46. 		bms.data[0] = s_bms.acc_x;	
  47. 		bms.data[1] = s_bms.acc_y;	
  48. 		bms.data[2] = s_bms.acc_z;	
  49. 		bms.data[3] = s_bms.gyro;
  50. 		bms.data[4] = s_bms.angle_acc;
  51. 		bms.data[5] = s_bms.angle_kalman;
  52. 		bat_pub.publish(bms);
  53.  
  54.  
  55. 		ros::spinOnce();
  56. 		loop_rate.sleep();
  57. 		
  58. 	}
  59. 	pthread_join(tid,NULL);
  60. }

can驱动部分代码，receiveFrame函数读取所有帧，通过帧ID 0x333 跳转到adis16450()
函数进行IMU原始数据以及融合数据的格式转换，并写入s_bms结构，用于消息发布，注意变量类型(此处必须为short 因为涉及负数数据)：

  1. #include <ros/ros.h>
  2. #include <memory>
  3. #include <string>
  4. #include <csignal>
  5. #include <geometry_msgs/Twist.h>
  6.  
  7. #include "can/can.h"
  8. #include "std_msgs/UInt8MultiArray.h"
  9. #include "std_msgs/MultiArrayDimension.h"
  10.  
  11. #define S_FLAG	0
  12. #define R_FLAG	1
  13.  
  14. double g_csb_dis[4] = {5.0,5.0};
  15.  
  16. static union {   
  17.     char c[4];
  18.     unsigned long l;
  19. }endian_test;
  20. #define ENDIANNESS ((char)endian_test.l)  
  21.  
  22. PVCI_CAN_OBJ frame;
  23. Quaternion q;
  24. S_BMS s_bms;
  25. S_MOTOR s_motor;
  26.  
  27. void adis16450(PVCI_CAN_OBJ frame)
  28. {
  29. 	float acc_x,acc_y,acc_z,gyro,k_angle,a_angle;
  30.  
  31. 	//short gryo_x_r = (frame->data[0]<<8) | frame->data[1];
  32. 	short acc_x_r = (frame->data[0]<<8) | frame->data[1];
  33. //	short acc_y_r = (frame->data[2]<<8) | frame->data[3];
  34. 	short acc_z_r = (frame->data[2]<<8) | frame->data[3];
  35. //	short gyro_r = (frame->data[6]<<8) | frame->data[7];
  36. 	union{
  37. 		float ram;
  38. 		unsigned char bytes[4];
  39. 	}thing;	
  40. 	thing.bytes[0] = frame->data[4];
  41. 	thing.bytes[1] = frame->data[5];
  42. 	thing.bytes[2] = frame->data[6];
  43. 	thing.bytes[3] = frame->data[7];
  44.  
  45.  
  46. 	acc_x = acc_x_r * 0.00025;
  47. //	acc_y = acc_y_r * 0.00025;
  48. 	acc_z = acc_z_r * 0.00025;
  49. //	gyro = gyro_r/40.0 + 0.124;
  50.  
  51. 	s_bms.acc_x = acc_x;
  52. //	s_bms.acc_y = acc_y;
  53. 	s_bms.acc_z = acc_z;
  54. 	s_bms.gyro = gyro;
  55. 	s_bms.angle_acc = a_angle;
  56. 	s_bms.angle_kalman = thing.ram;
  57.  
  58. 	//ROS_INFO("Q: %f %f",thing.ram,acc_x);
  59. }
  60.  
  61. void c620InfoProc(PVCI_CAN_OBJ frame)
  62. {
  63. 	s_motor.l_current = frame->data[1] | (frame->data[0]<<8);
  64. 	s_motor.r_current  =  frame->data[3] | (frame->data[2]<<8);
  65. 	s_motor.l_rpm = frame->data[5] | (frame->data[4]<<8);
  66. 	s_motor.r_rpm =  frame->data[7] | (frame->data[6]<<8);
  67.  //   ROS_INFO("RPM: %d %d",s_motor.l_rpm,s_motor.r_rpm);
  68. }
  69.  
  70. void bldcInfoProc(PVCI_CAN_OBJ frame)
  71. {
  72. 	int i = 0;
  73. 	double vl,vr,vxy;
  74. 	vl = frame->data[0] | (frame->data[1]<<8);
  75. 	vr = frame->data[2] | (frame->data[3]<<8);
  76. 	if(vl > 20000)
  77. 		vl -= 65536;
  78. 	if(vr > 20000)
  79. 		vr -= 65536;
  80. 	vl = vl/600 * 0.439822964;  // pi*d = 0.439822964
  81. 	vr = vr/600 * 0.439822964;
  82. 	vxy = (vr-vl) / 2.0 * -1.0;
  83. 	vth = (vr+vl) / 0.327 * -1.0;
  84. 	vx = cos(vth) * vxy;
  85. 	vy = -sin(vth) * vxy;
  86. }
  87.  
  88. void csbInfoProc(PVCI_CAN_OBJ frame)
  89. {
  90. 	g_csb_dis[0] = (frame->data[0]<<8 | frame->data[1]) / 1000.0;
  91. 	g_csb_dis[1] = (frame->data[2]<<8 | frame->data[3]) / 1000.0;
  92. } 
  93.  
  94. void batInfoProc(PVCI_CAN_OBJ frame)
  95. {	
  96. 	s_bms.coulomb = ((frame->data[0]<<8) | frame->data[1]) / 56470.0;
  97. 	s_bms.voltage = (70.8 * (frame->data[2]<<8 | frame->data[3]) / 65535.0);
  98. 	s_bms.current = frame->data[4]<<8 | frame->data[5];
  99. 	s_bms.current = 64.0 * (s_bms.current - 32767.0) / 32767.0;
  100. 	s_bms.temperature = 510.0 * (frame->data[6]<<8 | frame->data[7]) / 65535.0 - 273.0;	
  101. }
  102.  
  103. int printFrame(PVCI_CAN_OBJ frame,unsigned char flag)
  104. {
  105. 	int i = 0;
  106.     if(flag)
  107.         ROS_INFO("RX(0x%08X): ",frame->id);
  108.     else
  109.         ROS_INFO("TX(0x%08X): ",frame->id);
  110.  
  111.     if(frame->is_extended==0) ROS_INFO(" Standard ");
  112.     if(frame->is_extended==1) ROS_INFO(" Extend   ");
  113.     if(frame->is_rtr==0) ROS_INFO(" Data   ");
  114.     if(frame->is_rtr==1) ROS_INFO(" Remote ");
  115.     ROS_INFO("DLC:0x%02X  ",frame->dlc);
  116.  
  117.     for(i=0;i<(frame->dlc);i++)	
  118.         printf("%02X ",frame->data[i]);
  119. 	printf("\n");
  120.     return 1;
  121. }
  122.  
  123. void *receiveFrame(void *arg)
  124. {
  125.     int r_len = 0;
  126. 	int j = 0;
  127. 	int *run = (int*)arg;		
  128. 	
  129.     VCI_CAN_OBJ rec[2500];		// 接收缓存，设为3000为佳
  130.     if('b' == ENDIANNESS)		// big endian
  131.         ROS_INFO("Big endian");
  132.     else
  133.         ROS_INFO("Little endian");
  134.     while(*run & 0xff)
  135.     {
  136.         if((r_len = VCI_Receive(VCI_USBCAN2,0,0,rec,2500,100))>0)	//调用接收函数，如果有数据，进行数据处理显示。
  137.         {
  138.             for(j=0; j<r_len; j++)
  139.             {
  140. 				switch(rec[j].id)
  141. 				{
  142. 					case 0x382:
  143.                 		bldcInfoProc(&rec[j]);
  144. 						break;
  145. 					case 0x383:
  146. 						csbInfoProc(&rec[j]);
  147. 					//	printFrame(&rec[j],R_FLAG);
  148. 						break;
  149. 					case 0x377:
  150. 						batInfoProc(&rec[j]);
  151. 						break;
  152. 					case 0x332:
  153. 						c620InfoProc(&rec[j]);
  154. 						break;
  155. 					case 0x333:
  156. 						adis16450(&rec[j]);
  157. 						break;
  158. 					default:
  159. 						break;
  160. 				}
  161.             }
  162.         }
  163. 		usleep(10000);
  164.     }
  165. 	pthread_exit(0);
  166. }
  167.  
  168. int initFrame(PVCI_CAN_OBJ frame)	// 需要发送的帧，结构体设置
  169. {
  170.     int i = 0;
  171.     frame->id = 0x381;
  172.     frame->sendType = 0;
  173.     frame->is_rtr = 0;
  174.     frame->is_extended = 0;
  175.     frame->dlc=8;
  176.     for(i=0; i<(frame->dlc); i++)
  177.         frame->data[i] = 0;
  178.     return 1;
  179. }
  180.  
  181. int getCANInfo(unsigned int DevType)			//获取CAN设备信息
  182. {
  183.     int i = 0;
  184.     VCI_BOARD_INFO pInfo;
  185.     if(VCI_ReadBoardInfo(DevType,0,&pInfo)==1)//读取设备序列号、版本等信息。
  186.     {
  187.         ROS_INFO("Get device info success!");
  188. /*
  189.         printf("Serial Number:");
  190.         for(i=0; i<20; i++)
  191.             printf("%c", pInfo.str_Serial_Num[i]);
  192.         printf("\n");
  194.         printf("HW Type:");
  195.         for(i=0; i<10; i++)
  196.             printf("%c", pInfo.str_hw_Type[i]);
  197.         printf("\n");
  198. 	*/
  199.         return 1;
  200.     }
  201.     else
  202.     {
  203.         ROS_INFO("Get can info error!");
  204.         return 0;
  205.     }
  206. }
  207.  
  208. int sendCTLFrame(PVCI_CAN_OBJ frame)
  209. {
  210.     if(VCI_Transmit(VCI_USBCAN2, 0, 0, frame, 1) == 1)
  211.     {
  212.        // printFrame(frame,S_FLAG);
  213.         return 1;
  214.     }
  215.     else
  216.         return 0;
  217. }
  218.  
  219. int initCAN(unsigned int DevType)
  220. {
  221.     if(VCI_OpenDevice(VCI_USBCAN2,0,0) == 1)	//打开设备
  222.     {
  223.         ROS_INFO("Open CAN1 success!");
  224.     }
  225.     else
  226.     {
  227.         ROS_INFO("Open CAN1 error!");
  228.         return 0;
  229.     }
  230.     //初始化参数，严格参数二次开发函数库说明书
  231.     VCI_INIT_CONFIG config;
  232.     config.AccCode=0x80000000;
  233.     config.AccMask=0xFFFFFFFF;
  234.     config.Filter=1;        //接收所有帧
  235.     config.Timing0=0x00;    //波特率125 Kbps  0x03  0x1C
  236.     config.Timing1=0x1C;
  237.     config.Mode=0;          //正常模式
  238.  
  239.  
  240.     if(VCI_InitCAN(DevType,0,0,&config)!=1)
  241.     {
  242.         ROS_INFO("Init CAN1 error!");
  243.         VCI_CloseDevice(DevType,0);
  244.         return 0;
  245.     }
  246.     if(VCI_InitCAN(DevType,0,1,&config)!=1)
  247.     {
  248.         ROS_INFO("Init CAN2 error!");
  249.         VCI_CloseDevice(DevType,0);
  250.         return 0;
  251.     }
  252.     if(VCI_StartCAN(DevType,0,0)!=1)
  253.     {
  254.         ROS_INFO("Start CAN1 error!");
  255.         VCI_CloseDevice(DevType,0);
  256.         return 0;
  257.     }
  258.     if(VCI_StartCAN(DevType,0,1)!=1)
  259.     {
  260.         ROS_INFO("Start CAN2 error!");
  261.         VCI_CloseDevice(DevType,0);
  262.         return 0;
  263.     }
  264.     return 1;
  265. }
  266.  
  267. void closeCAN(PVCI_CAN_OBJ frame)
  268. {
  269.     VCI_ResetCAN(VCI_USBCAN2, 0, 0);	// 复位CAN1通道
  270.     usleep(100000);// delay 100ms
  271.     VCI_ResetCAN(VCI_USBCAN2, 0, 1);
  272.     usleep(100000);
  273.     VCI_CloseDevice(VCI_USBCAN2,0);		// 关闭设备
  274.     free(frame);
  275. }
  276.

头文件：

  1. #ifndef CAN_H
  2. #define CAN_H
  3.  
  4. //接口卡类型定义
  5. #define VCI_PCI5121		1
  6. #define VCI_PCI9810		2
  7. #define VCI_USBCAN1		3
  8. #define VCI_USBCAN2		4
  9. #define VCI_PCI9820		5
  10. #define VCI_CAN232		6
  11. #define VCI_PCI5110		7
  12. #define VCI_CANLite		8
  13. #define VCI_ISA9620		9
  14. #define VCI_ISA5420		10
  15.  
  16. //CAN错误码
  17. #define	ERR_CAN_OVERFLOW			0x0001	//CAN控制器内部FIFO溢出
  18. #define	ERR_CAN_ERRALARM			0x0002	//CAN控制器错误报警
  19. #define	ERR_CAN_PASSIVE				0x0004	//CAN控制器消极错误
  20. #define	ERR_CAN_LOSE				0x0008	//CAN控制器仲裁丢失
  21. #define	ERR_CAN_BUSERR				0x0010	//CAN控制器总线错误
  22.  
  23. //通用错误码
  24. #define	ERR_DEVICEOPENED			0x0100	//设备已经打开
  25. #define	ERR_DEVICEOPEN				0x0200	//打开设备错误
  26. #define	ERR_DEVICENOTOPEN			0x0400	//设备没有打开
  27. #define	ERR_BUFFEROVERFLOW			0x0800	//缓冲区溢出
  28. #define	ERR_DEVICENOTEXIST			0x1000	//此设备不存在
  29. #define	ERR_LOADKERNELDLL			0x2000	//装载动态库失败
  30. #define ERR_CMDFAILED				0x4000	//执行命令失败错误码
  31. #define	ERR_BUFFERCREATE			0x8000	//内存不足
  32.  
  33.  
  34. //函数调用返回状态值
  35. #define	STATUS_OK					1
  36. #define STATUS_ERR					0
  37. 	
  38. #define USHORT unsigned short int
  39. #define BYTE unsigned char
  40. #define CHAR char
  41. #define UCHAR unsigned char
  42. #define UINT unsigned int
  43. #define DWORD unsigned int
  44. #define PVOID void*
  45. #define ULONG unsigned int
  46. #define INT int
  47. #define UINT32 UINT
  48. #define LPVOID void*
  49. #define BOOL BYTE
  50. #define TRUE 1
  51. #define FALSE 0
  52.  
  53.  
  54. #if 1
  55. //1.ZLGCAN系列接口卡信息的数据类型。
  56. typedef  struct  _VCI_BOARD_INFO{
  57. 		USHORT	hw_Version;
  58. 		USHORT	fw_Version;
  59. 		USHORT	dr_Version;
  60. 		USHORT	in_Version;
  61. 		USHORT	irq_Num;
  62. 		BYTE	can_Num;
  63. 		CHAR	str_Serial_Num[20];
  64. 		CHAR	str_hw_Type[40];
  65. 		USHORT	Reserved[4];
  66. }VCI_BOARD_INFO,*PVCI_BOARD_INFO; 
  67.  
  68. //2.定义CAN信息帧的数据类型。
  69. typedef  struct  _VCI_CAN_OBJ{
  70. 	UINT	id;
  71. 	UINT	timeStamp;
  72. 	BYTE	timeFlag;
  73. 	BYTE	sendType;
  74. 	BYTE	is_rtr; 	 // 是否是远程帧
  75. 	BYTE	is_extended; // 是否是扩展帧
  76. 	BYTE	dlc;		 // data len
  77. 	BYTE	data[8];
  78. 	BYTE	reserved[3];
  79. }VCI_CAN_OBJ,*PVCI_CAN_OBJ;
  80.  
  81. //3.定义CAN控制器状态的数据类型。
  82. typedef struct _VCI_CAN_STATUS{
  83. 	UCHAR	ErrInterrupt;
  84. 	UCHAR	regMode;
  85. 	UCHAR	regStatus;
  86. 	UCHAR	regALCapture;
  87. 	UCHAR	regECCapture; 
  88. 	UCHAR	regEWLimit;
  89. 	UCHAR	regRECounter; 
  90. 	UCHAR	regTECounter;
  91. 	DWORD	Reserved;
  92. }VCI_CAN_STATUS,*PVCI_CAN_STATUS;
  93.  
  94. //4.定义错误信息的数据类型。
  95. typedef struct _ERR_INFO{
  96. 		UINT	ErrCode;
  97. 		BYTE	Passive_ErrData[3];
  98. 		BYTE	ArLost_ErrData;
  99. }VCI_ERR_INFO,*PVCI_ERR_INFO;
  100.  
  101. //5.定义初始化CAN的数据类型
  102. typedef struct _INIT_CONFIG{
  103. 	DWORD	AccCode;
  104. 	DWORD	AccMask;
  105. 	DWORD	Reserved;
  106. 	UCHAR	Filter;
  107. 	UCHAR	Timing0;	
  108. 	UCHAR	Timing1;	
  109. 	UCHAR	Mode;
  110. }VCI_INIT_CONFIG,*PVCI_INIT_CONFIG;
  111.  
  112.  
  113. struct Quaternion
  114. {
  115.     float w, x, y, z;
  116. 	float vx, vy, vz;
  117. 	float ax, ay, az;
  118. };
  119.  
  120. typedef struct bms{
  121. 	float_t voltage;
  122. 	float_t current;
  123. 	float_t temperature;
  124. 	float_t coulomb;
  125. 	float_t acc_x;	
  126. 	float_t acc_y;	
  127. 	float_t acc_z;	
  128. 	float_t gyro;
  129. 	float_t angle_acc;
  130. 	float_t angle_kalman;
  131. }S_BMS;
  132.  
  133. typedef struct motor{
  134. 	short l_rpm;
  135. 	short r_rpm;
  136. 	short l_current;
  137. 	short r_current;
  138. }S_MOTOR;
  139.  
  140.  
  141. struct EulerAngles 
  142. {
  143.     float roll, pitch, yaw;
  144. };
  145.  
  146. extern Quaternion q;
  147. extern P_V_INFO pVinfo;
  148. extern S_BMS s_bms;
  149. extern S_MOTOR s_motor;
  150.  
  151. extern void framePrint(unsigned int cid,unsigned char *buf);
  152. extern int initCAN(unsigned int DevType);
  153. extern void closeCAN(PVCI_CAN_OBJ frame);
  154. extern int getCANInfo(unsigned int DevType);
  155. extern void *receiveFrame(void *arg);
  156. extern void *can2RecvFrame(void *arg);
  157. extern int sendCTLFrame(PVCI_CAN_OBJ frame);
  158. extern double g_csb_dis[];
  159. extern PVCI_CAN_OBJ frame;
  160. extern double vx,vy,vth;
  161.  
  162. #ifdef __cplusplus
  163. extern "C" {
  164. #endif
  165.  
  166. DWORD VCI_OpenDevice(DWORD DeviceType,DWORD DeviceInd,DWORD Reserved);
  167. DWORD VCI_CloseDevice(DWORD DeviceType,DWORD DeviceInd);
  168. DWORD VCI_InitCAN(DWORD DeviceType, DWORD DeviceInd, DWORD CANInd, PVCI_INIT_CONFIG pInitConfig);
  169.  
  170. DWORD VCI_ReadBoardInfo(DWORD DeviceType,DWORD DeviceInd,PVCI_BOARD_INFO pInfo);
  171. DWORD VCI_ReadErrInfo(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_ERR_INFO pErrInfo);
  172. DWORD VCI_ReadCANStatus(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_STATUS pCANStatus);
  173.  
  174. DWORD VCI_GetReference(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,DWORD RefType,PVOID pData);
  175. DWORD VCI_SetReference(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,DWORD RefType,PVOID pData);
  176.  
  177. ULONG VCI_GetReceiveNum(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
  178. DWORD VCI_ClearBuffer(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
  179.  
  180. DWORD VCI_StartCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
  181. DWORD VCI_ResetCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
  182.  
  183. ULONG VCI_Transmit(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pSend,UINT Len);
  184. ULONG VCI_Receive(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pReceive,UINT Len,INT WaitTime);
  185.  
  186. #ifdef __cplusplus
  187. }
  188. #endif
  189. #endif
  190.  
  191. #endif

具体编译的时候请参考硬件厂家提供的资料，还有引入对应的库等，有的版本硬件驱动库还存在bug，本文章主要用作自己资料记录，仅贴出部分代码，如有技术方面问题可留言共同交流探讨，仅此而已。

最终效果图：

0cc900adee094481abc360e7cca0f97a.jpeg

 ec67b2d56b53472ba83173dd3dd383da.jpeg

31587efa7e644cdb96d579ed748d69c3.jpeg

完全调好之后的忘了拍照，贴几个随便拍的，最终效果在静止状态角度在正负0.005之间变化 （好像是的）。

 

 

 

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/2023面试高手/article/detail/703834
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号