├── .gitignore ├── ControlCAN.dll ├── ControlCAN.h ├── ControlCAN.lib ├── IMUFilter.pro ├── IMUFilter.pro.user ├── IMUfilter.cpp ├── IMUfilter.h ├── LICENSE ├── README.md ├── devicefilter.cpp ├── devicefilter.h ├── kerneldlls ├── CAN232.dll ├── CANETE.dll ├── CANET_TCP.dll ├── PC104C2.dll ├── PC104CAN.dll ├── PCI5121.dll ├── gisadll.dll ├── gpcidll.dll ├── isa5420.dll ├── kerneldll.ini └── usbcan.dll ├── macrodefs.h ├── main.cpp ├── mainwindow.cpp ├── mainwindow.h ├── mainwindow.ui ├── qcustomplot.cpp ├── qcustomplot.h ├── usbcan.cpp └── usbcan.h /.gitignore: -------------------------------------------------------------------------------- 1 | # C++ objects and libs 2 | 3 | *.slo 4 | *.lo 5 | *.o 6 | *.a 7 | *.la 8 | *.lai 9 | *.so 10 | *.dll 11 | *.dylib 12 | 13 | # Qt-es 14 | 15 | *.pro.user 16 | *.pro.user.* 17 | moc_*.cpp 18 | qrc_*.cpp 19 | Makefile 20 | *-build-* 21 | -------------------------------------------------------------------------------- /ControlCAN.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hustcalm/OpenIMUFilter/32c89faee3532642f1c7515bf644b8910e4d17d9/ControlCAN.dll -------------------------------------------------------------------------------- /ControlCAN.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTROLCAN_H 2 | #define CONTROLCAN_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_USBCAN2A 4 10 | #define VCI_PCI9820 5 11 | #define VCI_CAN232 6 12 | #define VCI_PCI5110 7 13 | #define VCI_CANLITE 8 14 | #define VCI_ISA9620 9 15 | #define VCI_ISA5420 10 16 | #define VCI_PC104CAN 11 17 | #define VCI_CANETUDP 12 18 | #define VCI_CANETE 12 19 | #define VCI_DNP9810 13 20 | #define VCI_PCI9840 14 21 | #define VCI_PC104CAN2 15 22 | #define VCI_PCI9820I 16 23 | #define VCI_CANETTCP 17 24 | #define VCI_PEC9920 18 25 | #define VCI_PCIE_9220 18 26 | #define VCI_PCI5010U 19 27 | #define VCI_USBCAN_E_U 20 28 | #define VCI_USBCAN_2E_U 21 29 | #define VCI_PCI5020U 22 30 | #define VCI_EG20T_CAN 23 31 | #define VCI_PCIE9221 24 32 | 33 | //CAN错误码 34 | #define ERR_CAN_OVERFLOW 0x0001 //CAN控制器内部FIFO溢出 35 | #define ERR_CAN_ERRALARM 0x0002 //CAN控制器错误报警 36 | #define ERR_CAN_PASSIVE 0x0004 //CAN控制器消极错误 37 | #define ERR_CAN_LOSE 0x0008 //CAN控制器仲裁丢失 38 | #define ERR_CAN_BUSERR 0x0010 //CAN控制器总线错误 39 | #define ERR_CAN_BUSOFF 0x0020 //总线关闭错误 40 | //通用错误码 41 | #define ERR_DEVICEOPENED 0x0100 //设备已经打开 42 | #define ERR_DEVICEOPEN 0x0200 //打开设备错误 43 | #define ERR_DEVICENOTOPEN 0x0400 //设备没有打开 44 | #define ERR_BUFFEROVERFLOW 0x0800 //缓冲区溢出 45 | #define ERR_DEVICENOTEXIST 0x1000 //此设备不存在 46 | #define ERR_LOADKERNELDLL 0x2000 //装载动态库失败 47 | #define ERR_CMDFAILED 0x4000 //执行命令失败错误码 48 | #define ERR_BUFFERCREATE 0x8000 //内存不足 49 | 50 | 51 | //函数调用返回状态值 52 | #define STATUS_OK 1 53 | #define STATUS_ERR 0 54 | 55 | #define CMD_DESIP 0 56 | #define CMD_DESPORT 1 57 | #define CMD_CHGDESIPANDPORT 2 58 | #define CMD_SRCPORT 2 59 | #define CMD_TCP_TYPE 4 //tcp 工作方式，服务器:1 或是客户端:0 60 | #define TCP_CLIENT 0 61 | #define TCP_SERVER 1 62 | //服务器方式下有效 63 | #define CMD_CLIENT_COUNT 5 //连接上的客户端计数 64 | #define CMD_CLIENTS 6 //连接上的客户端 65 | #define CMD_DISCONN_CLINET 7 //断开一个连接 66 | 67 | typedef struct tagRemoteClient{ 68 | int iIndex; 69 | DWORD port; 70 | HANDLE hClient; 71 | char szip[32]; 72 | }REMOTE_CLIENT; 73 | 74 | 75 | //1.ZLGCAN系列接口卡信息的数据类型。 76 | typedef struct _VCI_BOARD_INFO{ 77 | USHORT hw_Version; 78 | USHORT fw_Version; 79 | USHORT dr_Version; 80 | USHORT in_Version; 81 | USHORT irq_Num; 82 | BYTE can_Num; 83 | CHAR str_Serial_Num[20]; 84 | CHAR str_hw_Type[40]; 85 | USHORT Reserved[4]; 86 | } VCI_BOARD_INFO,*PVCI_BOARD_INFO; 87 | 88 | //2.定义CAN信息帧的数据类型。 89 | typedef struct _VCI_CAN_OBJ{ 90 | UINT ID; 91 | UINT TimeStamp; 92 | BYTE TimeFlag; 93 | BYTE SendType; 94 | BYTE RemoteFlag;//是否是远程帧 95 | BYTE ExternFlag;//是否是扩展帧 96 | BYTE DataLen; 97 | BYTE Data[8]; 98 | BYTE Reserved[3]; 99 | }VCI_CAN_OBJ,*PVCI_CAN_OBJ; 100 | 101 | //3.定义CAN控制器状态的数据类型。 102 | typedef struct _VCI_CAN_STATUS{ 103 | UCHAR ErrInterrupt; 104 | UCHAR regMode; 105 | UCHAR regStatus; 106 | UCHAR regALCapture; 107 | UCHAR regECCapture; 108 | UCHAR regEWLimit; 109 | UCHAR regRECounter; 110 | UCHAR regTECounter; 111 | DWORD Reserved; 112 | }VCI_CAN_STATUS,*PVCI_CAN_STATUS; 113 | 114 | //4.定义错误信息的数据类型。 115 | typedef struct _VCI_ERR_INFO{ 116 | UINT ErrCode; 117 | BYTE Passive_ErrData[3]; 118 | BYTE ArLost_ErrData; 119 | } VCI_ERR_INFO,*PVCI_ERR_INFO; 120 | 121 | //5.定义初始化CAN的数据类型 122 | typedef struct _VCI_INIT_CONFIG{ 123 | DWORD AccCode; 124 | DWORD AccMask; 125 | DWORD Reserved; 126 | UCHAR Filter; 127 | UCHAR Timing0; 128 | UCHAR Timing1; 129 | UCHAR Mode; 130 | }VCI_INIT_CONFIG,*PVCI_INIT_CONFIG; 131 | 132 | typedef struct _tagChgDesIPAndPort 133 | { 134 | char szpwd[10]; 135 | char szdesip[20]; 136 | int desport; 137 | BYTE blistenonly; 138 | }CHGDESIPANDPORT; 139 | 140 | ///////// new add struct for filter ///////// 141 | typedef struct _VCI_FILTER_RECORD{ 142 | DWORD ExtFrame; //是否为扩展帧 143 | DWORD Start; 144 | DWORD End; 145 | }VCI_FILTER_RECORD,*PVCI_FILTER_RECORD; 146 | 147 | 148 | #define EXTERNC extern "C" 149 | 150 | EXTERNC DWORD __stdcall VCI_OpenDevice(DWORD DeviceType,DWORD DeviceInd,DWORD Reserved); 151 | EXTERNC DWORD __stdcall VCI_CloseDevice(DWORD DeviceType,DWORD DeviceInd); 152 | EXTERNC DWORD __stdcall VCI_InitCAN(DWORD DeviceType, DWORD DeviceInd, DWORD CANInd, PVCI_INIT_CONFIG pInitConfig); 153 | 154 | EXTERNC DWORD __stdcall VCI_ReadBoardInfo(DWORD DeviceType,DWORD DeviceInd,PVCI_BOARD_INFO pInfo); 155 | EXTERNC DWORD __stdcall VCI_ReadErrInfo(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_ERR_INFO pErrInfo); 156 | EXTERNC DWORD __stdcall VCI_ReadCANStatus(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_STATUS pCANStatus); 157 | 158 | EXTERNC DWORD __stdcall VCI_GetReference(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,DWORD RefType,PVOID pData); 159 | EXTERNC DWORD __stdcall VCI_SetReference(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,DWORD RefType,PVOID pData); 160 | 161 | EXTERNC ULONG __stdcall VCI_GetReceiveNum(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd); 162 | EXTERNC DWORD __stdcall VCI_ClearBuffer(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd); 163 | 164 | EXTERNC DWORD __stdcall VCI_StartCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd); 165 | EXTERNC DWORD __stdcall VCI_ResetCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd); 166 | 167 | EXTERNC ULONG __stdcall VCI_Transmit(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pSend,ULONG Len); 168 | EXTERNC ULONG __stdcall VCI_Receive(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pReceive,ULONG Len,INT WaitTime=-1); 169 | 170 | 171 | #endif 172 | -------------------------------------------------------------------------------- /ControlCAN.lib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hustcalm/OpenIMUFilter/32c89faee3532642f1c7515bf644b8910e4d17d9/ControlCAN.lib -------------------------------------------------------------------------------- /IMUFilter.pro: -------------------------------------------------------------------------------- 1 | #------------------------------------------------- 2 | # 3 | # Project created by QtCreator 2013-11-15T10:24:08 4 | # 5 | #------------------------------------------------- 6 | 7 | QT += core gui 8 | 9 | greaterThan(QT_MAJOR_VERSION, 4): QT += widgets 10 | 11 | TARGET = IMUFilter 12 | TEMPLATE = app 13 | 14 | 15 | SOURCES += main.cpp\ 16 | mainwindow.cpp \ 17 | usbcan.cpp \ 18 | IMUfilter.cpp \ 19 | devicefilter.cpp \ 20 | qcustomplot.cpp 21 | 22 | HEADERS += mainwindow.h \ 23 | usbcan.h \ 24 | IMUfilter.h \ 25 | ControlCAN.h \ 26 | macrodefs.h \ 27 | devicefilter.h \ 28 | qcustomplot.h 29 | 30 | FORMS += mainwindow.ui 31 | 32 | unix|win32: LIBS += -L$$PWD/ -lControlCAN 33 | 34 | INCLUDEPATH += $$PWD/ 35 | DEPENDPATH += $$PWD/ 36 | -------------------------------------------------------------------------------- /IMUFilter.pro.user: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | ProjectExplorer.Project.ActiveTarget 7 | 0 8 | 9 | 10 | ProjectExplorer.Project.EditorSettings 11 | 12 | true 13 | false 14 | true 15 | 16 | Cpp 17 | 18 | CppGlobal 19 | 20 | 21 | 22 | QmlJS 23 | 24 | QmlJSGlobal 25 | 26 | 27 | 2 28 | UTF-8 29 | false 30 | 4 31 | false 32 | true 33 | 1 34 | true 35 | 0 36 | true 37 | 0 38 | 8 39 | true 40 | 1 41 | true 42 | true 43 | true 44 | false 45 | 46 | 47 | 48 | ProjectExplorer.Project.PluginSettings 49 | 50 | 51 | 52 | ProjectExplorer.Project.Target.0 53 | 54 | Desktop Qt 5.1.1 MSVC2012 32bit 55 | Desktop Qt 5.1.1 MSVC2012 32bit 56 | qt.511.win32_msvc2012.essentials_kit 57 | 1 58 | 0 59 | 0 60 | 61 | 62 | 63 | true 64 | qmake 65 | 66 | QtProjectManager.QMakeBuildStep 67 | false 68 | true 69 | 70 | false 71 | 72 | 73 | true 74 | Make 75 | 76 | Qt4ProjectManager.MakeStep 77 | 78 | false 79 | 80 | 81 | 82 | 2 83 | Build 84 | 85 | ProjectExplorer.BuildSteps.Build 86 | 87 | 88 | 89 | true 90 | Make 91 | 92 | Qt4ProjectManager.MakeStep 93 | 94 | true 95 | clean 96 | 97 | 98 | 1 99 | Clean 100 | 101 | ProjectExplorer.BuildSteps.Clean 102 | 103 | 2 104 | false 105 | 106 | Debug 107 | 108 | Qt4ProjectManager.Qt4BuildConfiguration 109 | 2 110 | G:/CAS/NLPR/Projects/IMU-F100A5/ZLG/DataProcesser/build-IMUFilter-Desktop_Qt_5_1_1_MSVC2012_32bit-Debug 111 | true 112 | 113 | 114 | 115 | 116 | true 117 | qmake 118 | 119 | QtProjectManager.QMakeBuildStep 120 | false 121 | true 122 | 123 | false 124 | 125 | 126 | true 127 | Make 128 | 129 | Qt4ProjectManager.MakeStep 130 | 131 | false 132 | 133 | 134 | 135 | 2 136 | Build 137 | 138 | ProjectExplorer.BuildSteps.Build 139 | 140 | 141 | 142 | true 143 | Make 144 | 145 | Qt4ProjectManager.MakeStep 146 | 147 | true 148 | clean 149 | 150 | 151 | 1 152 | Clean 153 | 154 | ProjectExplorer.BuildSteps.Clean 155 | 156 | 2 157 | false 158 | 159 | Release 160 | 161 | Qt4ProjectManager.Qt4BuildConfiguration 162 | 0 163 | G:/CAS/NLPR/Projects/IMU-F100A5/ZLG/DataProcesser/build-IMUFilter-Desktop_Qt_5_1_1_MSVC2012_32bit-Release 164 | true 165 | 166 | 2 167 | 168 | 169 | 0 170 | Deploy 171 | 172 | ProjectExplorer.BuildSteps.Deploy 173 | 174 | 1 175 | Deploy locally 176 | 177 | ProjectExplorer.DefaultDeployConfiguration 178 | 179 | 1 180 | 181 | 182 | true 183 | 184 | false 185 | false 186 | false 187 | false 188 | true 189 | 0.01 190 | 10 191 | true 192 | 25 193 | 194 | true 195 | valgrind 196 | 197 | 0 198 | 1 199 | 2 200 | 3 201 | 4 202 | 5 203 | 6 204 | 7 205 | 8 206 | 9 207 | 10 208 | 11 209 | 12 210 | 13 211 | 14 212 | 213 | 2 214 | 215 | IMUFilter 216 | 217 | Qt4ProjectManager.Qt4RunConfiguration:G:/CAS/NLPR/Projects/IMU-F100A5/ZLG/DataProcesser/IMUFilter/IMUFilter.pro 218 | 219 | IMUFilter.pro 220 | false 221 | false 222 | 223 | 3768 224 | true 225 | false 226 | false 227 | false 228 | true 229 | 230 | 1 231 | 232 | 233 | 234 | ProjectExplorer.Project.TargetCount 235 | 1 236 | 237 | 238 | ProjectExplorer.Project.Updater.EnvironmentId 239 | {85f47755-6752-4b55-9938-afe90e95b9c6} 240 | 241 | 242 | ProjectExplorer.Project.Updater.FileVersion 243 | 14 244 | 245 | 246 | -------------------------------------------------------------------------------- /IMUfilter.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @author Aaron Berk 3 | * 4 | * @section LICENSE 5 | * 6 | * Copyright (c) 2010 ARM Limited 7 | * 8 | * Permission is hereby granted, free of charge, to any person obtaining a copy 9 | * of this software and associated documentation files (the "Software"), to deal 10 | * in the Software without restriction, including without limitation the rights 11 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | * copies of the Software, and to permit persons to whom the Software is 13 | * furnished to do so, subject to the following conditions: 14 | * 15 | * The above copyright notice and this permission notice shall be included in 16 | * all copies or substantial portions of the Software. 17 | * 18 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 24 | * THE SOFTWARE. 25 | * 26 | * @section DESCRIPTION 27 | * 28 | * IMU orientation filter developed by Sebastian Madgwick. 29 | * 30 | * Find more details about his paper here: 31 | * 32 | * http://code.google.com/p/imumargalgorithm30042010sohm/ 33 | */ 34 | 35 | /** 36 | * Includes 37 | */ 38 | #include "IMUfilter.h" 39 | 40 | IMUfilter::IMUfilter(double rate, double gyroscopeMeasurementError){ 41 | 42 | firstUpdate = 0; 43 | 44 | //Quaternion orientation of earth frame relative to auxiliary frame. 45 | AEq_1 = 1; 46 | AEq_2 = 0; 47 | AEq_3 = 0; 48 | AEq_4 = 0; 49 | 50 | //Estimated orientation quaternion elements with initial conditions. 51 | SEq_1 = 1; 52 | SEq_2 = 0; 53 | SEq_3 = 0; 54 | SEq_4 = 0; 55 | 56 | //Sampling period (typical value is ~0.1s). 57 | deltat = rate; 58 | 59 | //Gyroscope measurement error (in degrees per second). 60 | gyroMeasError = gyroscopeMeasurementError; 61 | 62 | //Compute beta. 63 | beta = sqrt(3.0 / 4.0) * (PI * (gyroMeasError / 180.0)); 64 | 65 | } 66 | 67 | void IMUfilter::updateFilter(double w_x, double w_y, double w_z, double a_x, double a_y, double a_z) { 68 | 69 | //Local system variables. 70 | 71 | //Vector norm. 72 | double norm; 73 | //Quaternion rate from gyroscope elements. 74 | double SEqDot_omega_1; 75 | double SEqDot_omega_2; 76 | double SEqDot_omega_3; 77 | double SEqDot_omega_4; 78 | //Objective function elements. 79 | double f_1; 80 | double f_2; 81 | double f_3; 82 | //Objective function Jacobian elements. 83 | double J_11or24; 84 | double J_12or23; 85 | double J_13or22; 86 | double J_14or21; 87 | double J_32; 88 | double J_33; 89 | //Objective function gradient elements. 90 | double nablaf_1; 91 | double nablaf_2; 92 | double nablaf_3; 93 | double nablaf_4; 94 | 95 | //Auxiliary variables to avoid reapeated calcualtions. 96 | double halfSEq_1 = 0.5 * SEq_1; 97 | double halfSEq_2 = 0.5 * SEq_2; 98 | double halfSEq_3 = 0.5 * SEq_3; 99 | double halfSEq_4 = 0.5 * SEq_4; 100 | double twoSEq_1 = 2.0 * SEq_1; 101 | double twoSEq_2 = 2.0 * SEq_2; 102 | double twoSEq_3 = 2.0 * SEq_3; 103 | 104 | //Compute the quaternion rate measured by gyroscopes. 105 | SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z; 106 | SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y; 107 | SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x; 108 | SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x; 109 | 110 | //Normalise the accelerometer measurement. 111 | norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z); 112 | a_x /= norm; 113 | a_y /= norm; 114 | a_z /= norm; 115 | 116 | //Compute the objective function and Jacobian. 117 | f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x; 118 | f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y; 119 | f_3 = 1.0 - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z; 120 | //J_11 negated in matrix multiplication. 121 | J_11or24 = twoSEq_3; 122 | J_12or23 = 2 * SEq_4; 123 | //J_12 negated in matrix multiplication 124 | J_13or22 = twoSEq_1; 125 | J_14or21 = twoSEq_2; 126 | //Negated in matrix multiplication. 127 | J_32 = 2 * J_14or21; 128 | //Negated in matrix multiplication. 129 | J_33 = 2 * J_11or24; 130 | 131 | //Compute the gradient (matrix multiplication). 132 | nablaf_1 = J_14or21 * f_2 - J_11or24 * f_1; 133 | nablaf_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3; 134 | nablaf_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1; 135 | nablaf_4 = J_14or21 * f_1 + J_11or24 * f_2; 136 | 137 | //Normalise the gradient. 138 | norm = sqrt(nablaf_1 * nablaf_1 + nablaf_2 * nablaf_2 + nablaf_3 * nablaf_3 + nablaf_4 * nablaf_4); 139 | nablaf_1 /= norm; 140 | nablaf_2 /= norm; 141 | nablaf_3 /= norm; 142 | nablaf_4 /= norm; 143 | 144 | //Compute then integrate the estimated quaternion rate. 145 | SEq_1 += (SEqDot_omega_1 - (beta * nablaf_1)) * deltat; 146 | SEq_2 += (SEqDot_omega_2 - (beta * nablaf_2)) * deltat; 147 | SEq_3 += (SEqDot_omega_3 - (beta * nablaf_3)) * deltat; 148 | SEq_4 += (SEqDot_omega_4 - (beta * nablaf_4)) * deltat; 149 | 150 | //Normalise quaternion 151 | norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4); 152 | SEq_1 /= norm; 153 | SEq_2 /= norm; 154 | SEq_3 /= norm; 155 | SEq_4 /= norm; 156 | 157 | if (firstUpdate == 0) { 158 | //Store orientation of auxiliary frame. 159 | AEq_1 = SEq_1; 160 | AEq_2 = SEq_2; 161 | AEq_3 = SEq_3; 162 | AEq_4 = SEq_4; 163 | firstUpdate = 1; 164 | } 165 | 166 | } 167 | 168 | void IMUfilter::computeEuler(void){ 169 | 170 | //Quaternion describing orientation of sensor relative to earth. 171 | double ESq_1, ESq_2, ESq_3, ESq_4; 172 | //Quaternion describing orientation of sensor relative to auxiliary frame. 173 | double ASq_1, ASq_2, ASq_3, ASq_4; 174 | 175 | //Compute the quaternion conjugate. 176 | ESq_1 = SEq_1; 177 | ESq_2 = -SEq_2; 178 | ESq_3 = -SEq_3; 179 | ESq_4 = -SEq_4; 180 | 181 | //Compute the quaternion product. 182 | ASq_1 = ESq_1 * AEq_1 - ESq_2 * AEq_2 - ESq_3 * AEq_3 - ESq_4 * AEq_4; 183 | ASq_2 = ESq_1 * AEq_2 + ESq_2 * AEq_1 + ESq_3 * AEq_4 - ESq_4 * AEq_3; 184 | ASq_3 = ESq_1 * AEq_3 - ESq_2 * AEq_4 + ESq_3 * AEq_1 + ESq_4 * AEq_2; 185 | ASq_4 = ESq_1 * AEq_4 + ESq_2 * AEq_3 - ESq_3 * AEq_2 + ESq_4 * AEq_1; 186 | 187 | //Compute the Euler angles from the quaternion. 188 | phi = atan2(2 * ASq_3 * ASq_4 - 2 * ASq_1 * ASq_2, 2 * ASq_1 * ASq_1 + 2 * ASq_4 * ASq_4 - 1); 189 | theta = asin(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_3); 190 | psi = atan2(2 * ASq_2 * ASq_3 - 2 * ASq_1 * ASq_4, 2 * ASq_1 * ASq_1 + 2 * ASq_2 * ASq_2 - 1); 191 | 192 | } 193 | 194 | double IMUfilter::getRoll(void){ 195 | 196 | return phi; 197 | 198 | } 199 | 200 | double IMUfilter::getPitch(void){ 201 | 202 | return theta; 203 | 204 | } 205 | 206 | double IMUfilter::getYaw(void){ 207 | 208 | return psi; 209 | 210 | } 211 | 212 | // Degrees 213 | double IMUfilter::getRollInDegrees(void){ 214 | 215 | return toDegrees(phi); 216 | 217 | } 218 | 219 | double IMUfilter::getPitchInDegrees(void){ 220 | 221 | return toDegrees(theta); 222 | 223 | } 224 | 225 | double IMUfilter::getYawInDegrees(void){ 226 | 227 | return toDegrees(psi); 228 | 229 | } 230 | 231 | void IMUfilter::reset(void) { 232 | 233 | firstUpdate = 0; 234 | 235 | //Quaternion orientation of earth frame relative to auxiliary frame. 236 | AEq_1 = 1; 237 | AEq_2 = 0; 238 | AEq_3 = 0; 239 | AEq_4 = 0; 240 | 241 | //Estimated orientation quaternion elements with initial conditions. 242 | SEq_1 = 1; 243 | SEq_2 = 0; 244 | SEq_3 = 0; 245 | SEq_4 = 0; 246 | 247 | } 248 | -------------------------------------------------------------------------------- /IMUfilter.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @author Aaron Berk 3 | * 4 | * @section LICENSE 5 | * 6 | * Copyright (c) 2010 ARM Limited 7 | * 8 | * Permission is hereby granted, free of charge, to any person obtaining a copy 9 | * of this software and associated documentation files (the "Software"), to deal 10 | * in the Software without restriction, including without limitation the rights 11 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 12 | * copies of the Software, and to permit persons to whom the Software is 13 | * furnished to do so, subject to the following conditions: 14 | * 15 | * The above copyright notice and this permission notice shall be included in 16 | * all copies or substantial portions of the Software. 17 | * 18 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 19 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 20 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 21 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 22 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 23 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 24 | * THE SOFTWARE. 25 | * 26 | * @section DESCRIPTION 27 | * 28 | * IMU orientation filter developed by Sebastian Madgwick. 29 | * 30 | * Find more details about his paper here: 31 | * 32 | * http://code.google.com/p/imumargalgorithm30042010sohm/ 33 | */ 34 | 35 | #ifndef IMU_FILTER_H 36 | #define IMU_FILTER_H 37 | 38 | /** 39 | * Includes 40 | */ 41 | 42 | #include 43 | #include "macrodefs.h" 44 | 45 | /** 46 | * Defines 47 | */ 48 | #define PI 3.1415926536 49 | 50 | /** 51 | * IMU orientation filter. 52 | */ 53 | class IMUfilter { 54 | 55 | public: 56 | 57 | /** 58 | * Constructor. 59 | * 60 | * Initializes filter variables. 61 | * 62 | * @param rate The rate at which the filter should be updated. 63 | * @param gyroscopeMeasurementError The error of the gyroscope in degrees 64 | * per second. This used to calculate a tuning constant for the filter. 65 | * Try changing this value if there are jittery readings, or they change 66 | * too much or too fast when rotating the IMU. 67 | */ 68 | IMUfilter(double rate, double gyroscopeMeasurementError); 69 | 70 | /** 71 | * Update the filter variables. 72 | * 73 | * @param w_x X-axis gyroscope reading in rad/s. 74 | * @param w_y Y-axis gyroscope reading in rad/s. 75 | * @param w_z Z-axis gyroscope reading in rad/s. 76 | * @param a_x X-axis accelerometer reading in m/s/s. 77 | * @param a_y Y-axis accelerometer reading in m/s/s. 78 | * @param a_z Z-axis accelerometer reading in m/s/s. 79 | */ 80 | void updateFilter(double w_x, double w_y, double w_z, 81 | double a_x, double a_y, double a_z); 82 | 83 | /** 84 | * Compute the Euler angles based on the current filter data. 85 | */ 86 | void computeEuler(void); 87 | 88 | /** 89 | * Get the current roll. 90 | * 91 | * @return The current roll angle in radians. 92 | */ 93 | double getRoll(void); 94 | 95 | /** 96 | * Get the current roll. 97 | * 98 | * @return The current roll angle in degrees. 99 | */ 100 | double getRollInDegrees(void); 101 | 102 | /** 103 | * Get the current pitch. 104 | * 105 | * @return The current pitch angle in radians. 106 | */ 107 | double getPitch(void); 108 | 109 | /** 110 | * Get the current pitch. 111 | * 112 | * @return The current pitch angle in degrees. 113 | */ 114 | double getPitchInDegrees(void); 115 | 116 | /** 117 | * Get the current yaw. 118 | * 119 | * @return The current yaw angle in radians. 120 | */ 121 | double getYaw(void); 122 | 123 | /** 124 | * Get the current yaw. 125 | * 126 | * @return The current yaw angle in degrees. 127 | */ 128 | double getYawInDegrees(void); 129 | 130 | /** 131 | * Reset the filter. 132 | */ 133 | void reset(void); 134 | 135 | private: 136 | 137 | int firstUpdate; 138 | 139 | //Quaternion orientation of earth frame relative to auxiliary frame. 140 | double AEq_1; 141 | double AEq_2; 142 | double AEq_3; 143 | double AEq_4; 144 | 145 | //Estimated orientation quaternion elements with initial conditions. 146 | double SEq_1; 147 | double SEq_2; 148 | double SEq_3; 149 | double SEq_4; 150 | 151 | //Sampling period 152 | double deltat; 153 | 154 | //Gyroscope measurement error (in degrees per second). 155 | double gyroMeasError; 156 | 157 | //Compute beta (filter tuning constant.. 158 | double beta; 159 | 160 | double phi; 161 | double theta; 162 | double psi; 163 | 164 | }; 165 | 166 | #endif /* IMU_FILTER_H */ 167 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2013 Lihang Li 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software is furnished to do so, 10 | subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 17 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | OpenIMUFilter 2 | ============= 3 | 4 | A Project aimed to demo filters for IMU(the complementary filter, the Kalman filter and the Mahony&Madgwick filter) with lots of references and tutorials. 5 | 6 | ## References 7 | ### IMU 8 | #### Tutorials And Algorithms 9 | * [A Guide To using IMU (Accelerometer and Gyroscope Devices) in Embedded Applications](http://www.starlino.com/imu_guide.html) 10 | * [Arduino code for IMU Guide algorithm. Using a 5DOF IMU (accelerometer and gyroscope combo)](http://www.starlino.com/imu_kalman_arduino.html) 11 | * [DCM Tutorial – An Introduction to Orientation Kinematics](http://www.starlino.com/dcm_tutorial.html) 12 | * [IMU Data Fusing: Complementary, Kalman, and Mahony Filter](http://www.olliw.eu/2013/imu-data-fusing/) 13 | 14 | #### IMU Products 15 | * [x-IMU From X-IO Technologies](http://www.x-io.co.uk/products/x-imu/#!prettyPhoto) 16 | * [IMU Brick - TinkerForge](http://www.tinkerforge.com/en/doc/Hardware/Bricks/IMU_Brick.html) 17 | * [New Open source IMU - PIXHAWK](https://pixhawk.ethz.ch/blog/new_open_source_imu_inertial_measurement_unit) 18 | * [mbed cookbook for IMU](http://mbed.org/cookbook/IMU) 19 | 20 | #### Source Code 21 | * [Open source IMU and AHRS algorithms From X-IO Technologies](http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/) 22 | * [Orientation estimation algorithms for IMUs (accelerometers and gyroscopes) and MARG sensor arrays (accelerometers, gyroscopes and magnetometers)](https://code.google.com/p/imumargalgorithm30042010sohm/) 23 | * [mbed IMUfilter](http://mbed.org/users/aberk/code/IMUfilter/) 24 | * [Example-Sketch-for-IMU-including-Kalman-filter From TKJElectronics](https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter) 25 | * [KalmanFilter From TKJElectronics](https://github.com/TKJElectronics/KalmanFilter) 26 | * [openahrs](https://github.com/cbecker/openahrs) 27 | 28 | #### Calibration 29 | * [Industrial Grade Sensor Calibration From VectorNav](http://www.vectornav.com/support/library?id=86) 30 | 31 | ### Qt 32 | #### Timers 33 | * [Timers - Qt 4.8 Docs](http://qt-project.org/doc/qt-4.8/timers.html) 34 | 35 | #### QThread 36 | * [How To Really, Truly Use QThreads; The Full Explanation](http://mayaposch.wordpress.com/2011/11/01/how-to-really-truly-use-qthreads-the-full-explanation/) 37 | * [How to Use QThread in the Right Way (Part 1)](http://blog.debao.me/2013/08/how-to-use-qthread-in-the-right-way-part-1/) 38 | * [How to Use QThread in the Right Way (Part 2)](http://blog.debao.me/2013/08/how-to-use-qthread-in-the-right-way-part-2/) 39 | * [Qt thread: simple, complete and stable (with full sources on GitHub)](http://fabienpn.wordpress.com/2013/05/01/qt-thread-simple-and-stable-with-sources/) 40 | * [纠正你的QThread 的使用方法](http://blog.csdn.net/hustyangju/article/details/9491639) 41 | 42 | #### Plotter 43 | * [QCustomPlot](http://www.qcustomplot.com/index.php/introduction) 44 | 45 | #### Console Application 46 | * [QT Console Application Template Tutorial](http://treyweaver.blogspot.com/2013/02/qt-console-application-template-tutorial.html) 47 | * [How to write a nice console application with Qt and Qt Creator](http://www.lubby.org/ebooks/qtconsoleapp2/qtconsoleapp2.html) 48 | * [A non-GUI example - Qt Docs](http://doc.qt.digia.com/solutions/4/qtsingleapplication/qtsinglecoreapplication-example-console.html) 49 | 50 | ### C++ 51 | #### Exception Handling 52 | * [Understanding C++ Exception Handling](http://www.gamedev.net/page/resources/_/technical/general-programming/understanding-c-exception-handling-r953) 53 | * [C++处理异常技巧-try,catch,throw,finally](http://blog.csdn.net/renwotao2009/article/details/6751687) 54 | 55 | #### Floating Number Exception 56 | * [打印浮点数输出 1.#IND 1.#INF nan inf 1.#QNAN SNaN等解决](http://hayyoungsue.blog.163.com/blog/static/12384097720123662438501/) 57 | * [浮点异常值：NAN，QNAN，SNAN](http://www.cnblogs.com/konlil/archive/2011/07/06/2099646.html) 58 | -------------------------------------------------------------------------------- /devicefilter.cpp: -------------------------------------------------------------------------------- 1 | #include "devicefilter.h" 2 | 3 | deviceFilter::deviceFilter() 4 | { 5 | // Initialization for variables 6 | showVerboseInfo = FALSE; 7 | 8 | totalReceivedFrames = 0; 9 | totalReceivedID66 = 0; 10 | totalReceivedID67 = 0; 11 | totalReceivedID69 = 0; 12 | 13 | // Initialization for Accel and Gyro 14 | std::cout << "Now Initializing Accel..." << std::endl; 15 | initializeAcceleromter(); 16 | std::cout << "Done!" << endl; 17 | 18 | std::cout << "Now Initializing Gyro..." << std::endl; 19 | initializeGyroscope(); 20 | std::cout << "Done!" << std::endl; 21 | 22 | std::cout << "Now calibrating Gyro and Accel for Zeor Offset.." << std::endl; 23 | //calibrateGyroAndAccel(); 24 | std::cout << "Done!" << std::endl; 25 | 26 | _sampleTimer = new QTimer(this); 27 | connect(_sampleTimer, SIGNAL(timeout()), this, SLOT(sampleGyroAndAccel())); 28 | //connect(_sampleTimer, SIGNAL(timeout()), this, SLOT(testSampleTimer())); 29 | 30 | _filterTimer = new QTimer(this); 31 | //connect(_filterTimer, SIGNAL(timeout()), this, SLOT(naiveIntegrationFilter())); 32 | connect(_filterTimer, SIGNAL(timeout()), this, SLOT(filterUpdateAndCalc())); 33 | //connect(_filterTimer, SIGNAL(timeout()), this, SLOT(testFilterTimer())); 34 | 35 | _sampleTimer->start(TIMER_INTERVAL); 36 | _filterTimer->start(TIMER_INTERVAL); 37 | } 38 | 39 | deviceFilter::~deviceFilter() 40 | { 41 | if(_sampleTimer){ 42 | delete _sampleTimer; 43 | } 44 | 45 | if(_filterTimer){ 46 | delete _filterTimer; 47 | } 48 | 49 | if(_usbCANDevice){ 50 | if(_usbCANDevice->closeDevice()){ 51 | std::cout << "Device Closed!" << endl; 52 | } 53 | else{ 54 | _usbCANDevice->showErrorInfo(); 55 | } 56 | 57 | delete _usbCANDevice; 58 | } 59 | if(_imuFilter){ 60 | delete _imuFilter; 61 | } 62 | } 63 | 64 | void deviceFilter::sampleGyroAndAccel() 65 | { 66 | w_xAccumulator = 0.0; 67 | w_yAccumulator = 0.0; 68 | w_zAccumulator = 0.0; 69 | 70 | a_xAccumulator = 0.0; 71 | a_yAccumulator = 0.0; 72 | a_zAccumulator = 0.0; 73 | 74 | totalReceivedID66 = 0; 75 | totalReceivedID67 = 0; 76 | totalReceivedID69 = 0; 77 | 78 | if(_usbCANDevice->getReceiveNumber()){ 79 | recNumber = _usbCANDevice->getUnreadFramesNumber(); 80 | } 81 | else{ 82 | _usbCANDevice->showErrorInfo(); 83 | } 84 | 85 | if(_usbCANDevice->receiveData(vco, recNumber)){ 86 | receivedFrames = _usbCANDevice->getReveivedFramsNumber(); 87 | totalReceivedFrames += receivedFrames; 88 | 89 | for(unsigned int i = 0; i < receivedFrames; i++){ 90 | if(vco[i].ID == GYRO_X_Y){ 91 | totalReceivedID66++; 92 | 93 | // Gyro X and Y Rate in Raw format 94 | gyroXLong = vco[i].Data[3] << 24 | vco[i].Data[2] << 16 | vco[i].Data[1] << 8 | vco[i].Data[0]; 95 | gyroYLong = vco[i].Data[7] << 24 | vco[i].Data[6] << 16 | vco[i].Data[5] << 8 | vco[i].Data[4]; 96 | 97 | // Real Gyro X and Y Rate 98 | gyroRateX = gyroXLong * GYRO_RATE_LSB; 99 | gyroRateY = gyroYLong * GYRO_RATE_LSB; 100 | 101 | // Accumulator 102 | w_xAccumulator += gyroRateX; 103 | w_yAccumulator += gyroRateY; 104 | 105 | //w_xAccumulator += gyroRateX - w_xBias; 106 | //w_yAccumulator += gyroRateY - w_yBias; 107 | } 108 | else if(vco[i].ID == GYRO_Z_Temp){ 109 | totalReceivedID67++; 110 | 111 | // Gyro Z Rate in Raw format 112 | gyroZLong = vco[i].Data[3] << 24 | vco[i].Data[2] << 16 | vco[i].Data[1] << 8 | vco[i].Data[0]; 113 | tempOfIMULong = vco[i].Data[7]; 114 | 115 | // Real Gyro Z Rate 116 | gyroRateZ = gyroZLong * GYRO_RATE_LSB; 117 | tempIMU = tempOfIMULong * TEMP_IMU_LSB; 118 | 119 | // Accumulator 120 | w_zAccumulator += gyroRateZ; 121 | 122 | //w_zAccumulator += gyroRateZ - w_zBias; 123 | } 124 | else if(vco[i].ID == ACCEL_X_Y_Z){ 125 | totalReceivedID69++; 126 | 127 | // Accel X, Y, Z Rate in Raw format 128 | accelXLong = vco[i].Data[1] << 8 | vco[i].Data[0]; 129 | accelYLong = vco[i].Data[3] << 8 | vco[i].Data[2]; 130 | accelZLong = vco[i].Data[5] << 8 | vco[i].Data[4]; 131 | 132 | // Real Accel X, Y, Z Rate 133 | accelRateX = accelXLong * ACCEL_RATE_LSB; 134 | accelRateY = accelYLong * ACCEL_RATE_LSB; 135 | accelRateZ = accelZLong * ACCEL_RATE_LSB; 136 | 137 | // Accumulator 138 | a_xAccumulator += accelRateX; 139 | a_yAccumulator += accelRateY; 140 | a_zAccumulator += accelRateZ; 141 | 142 | //a_xAccumulator += accelRateX - a_xBias; 143 | //a_yAccumulator += accelRateY - a_yBias; 144 | //a_zAccumulator += accelRateZ - a_zBias; 145 | } 146 | else{ 147 | std::cout << "No defined frame ID received!" << std::endl; 148 | } 149 | } 150 | 151 | // Return the averaged rates 152 | 153 | // degree/s 154 | if(totalReceivedID66 != 0){ 155 | w_x = w_xAccumulator/totalReceivedID66; 156 | w_y = w_yAccumulator/totalReceivedID66; 157 | 158 | w_x_last = w_x; 159 | w_y_last = w_y; 160 | } 161 | else{ 162 | w_x = w_x_last; 163 | w_y = w_y_last; 164 | } 165 | 166 | if(totalReceivedID67 != 0){ 167 | w_z = w_zAccumulator/totalReceivedID67; 168 | 169 | w_z_last = w_z; 170 | } 171 | else{ 172 | w_z = w_z_last; 173 | } 174 | 175 | // rad/s 176 | w_x_rad_s = toRadians(w_x); 177 | w_y_rad_s = toRadians(w_y); 178 | w_z_rad_s = toRadians(w_z); 179 | 180 | // g 181 | if(totalReceivedID69 != 0){ 182 | a_x = a_xAccumulator/totalReceivedID69; 183 | a_y = a_yAccumulator/totalReceivedID69; 184 | a_z = a_zAccumulator/totalReceivedID69; 185 | } 186 | else{ 187 | a_x = a_x_last; 188 | a_y = a_y_last; 189 | a_z = a_z_last; 190 | } 191 | 192 | // m/s/s 193 | a_x_m_ss = a_x * g0; 194 | a_y_m_ss = a_y * g0; 195 | a_z_m_ss = a_z * g0; 196 | 197 | /* 198 | cout << w_x << " " 199 | << w_y << " " 200 | << w_z << " " 201 | << a_x << " " 202 | << a_y << " " 203 | << a_z << endl; 204 | */ 205 | 206 | emit postGyroAndAccelData(w_x, w_y, w_z, a_x, a_y, a_z); 207 | 208 | } 209 | else{ 210 | _usbCANDevice->showErrorInfo(); 211 | } 212 | } 213 | 214 | void deviceFilter::filterUpdateAndCalc() 215 | { 216 | //Sample Gyro and Accel Data 217 | //sampleGyroAndAccel(); 218 | 219 | // Update the filter variables 220 | _imuFilter->updateFilter(w_x_rad_s, w_y_rad_s, w_z_rad_s, a_x_m_ss, a_y_m_ss, a_z_m_ss); 221 | _imuFilter->computeEuler(); 222 | showVerboseInfo && 223 | std::cout << "Pitch: " << _imuFilter->getPitch() << " " 224 | << "Yaw: " << _imuFilter->getYaw() << " " 225 | << "Roll: " << _imuFilter->getRoll() << std::endl; 226 | 227 | emit postFilterOrientationData(_imuFilter->getPitchInDegrees(), _imuFilter->getYawInDegrees(), _imuFilter->getRollInDegrees()); 228 | } 229 | 230 | void deviceFilter::naiveIntegrationFilter() 231 | { 232 | // Just do simple and stupid integration 233 | gyroOrientationX = gyroOrientationX + w_x * FILTER_RATE; 234 | gyroOrientationY = gyroOrientationY + w_y * FILTER_RATE; 235 | gyroOrientationZ = gyroOrientationZ + w_z * FILTER_RATE; 236 | 237 | // Debug for NAN 238 | //gyroOrientationX = gyroOrientationY = gyroOrientationZ = 90.0; 239 | 240 | cout << gyroOrientationX << " " << gyroOrientationY << " " << gyroOrientationZ << endl; 241 | emit postFilterOrientationData(gyroOrientationX, gyroOrientationY, gyroOrientationZ); 242 | 243 | } 244 | 245 | void deviceFilter::attachUsbCANDevice(USBCAN* usbCANDevice) 246 | { 247 | this->_usbCANDevice = usbCANDevice; 248 | } 249 | 250 | void deviceFilter::attachImuFilter(IMUfilter* imuFilter) 251 | { 252 | this->_imuFilter = imuFilter; 253 | } 254 | 255 | void deviceFilter::testSampleTimer() 256 | { 257 | std::cout << "sampleTimer Triggered!" << std::endl; 258 | } 259 | 260 | void deviceFilter::testFilterTimer() 261 | { 262 | std::cout << "filterTimer Triggered!" << std::endl; 263 | } 264 | 265 | void deviceFilter::initializeAcceleromter() 266 | { 267 | // Initialization of Accel 268 | 269 | a_x = 0.0; 270 | a_y = 0.0; 271 | a_z = 0.0; 272 | 273 | a_x_last = 0.0; 274 | a_y_last = 0.0; 275 | a_z_last = 0.0; 276 | 277 | a_x_m_ss = 0.0; 278 | a_y_m_ss = 0.0; 279 | a_z_m_ss = 0.0; 280 | 281 | a_xBias = 0.0; 282 | a_yBias = 0.0; 283 | a_zBias = 0.0; 284 | 285 | a_xAccumulator = 0.0; 286 | a_yAccumulator = 0.0; 287 | a_zAccumulator = 0.0; 288 | 289 | accelXLong = 0; 290 | accelYLong = 0; 291 | accelZLong = 0; 292 | 293 | accelRateX = 0.0; 294 | accelRateY = 0.0; 295 | accelRateZ = 0.0; 296 | } 297 | 298 | void deviceFilter::calibrateAccelerometer() 299 | { 300 | // Calibration of Accel to calculate the Bias 301 | std::cout << "Please use calibrateGyroAndAccel instead!" << std::endl; 302 | } 303 | 304 | void deviceFilter::initializeGyroscope() 305 | { 306 | // Initialization of Gyro 307 | 308 | w_x = 0.0; 309 | w_y = 0.0; 310 | w_z = 0.0; 311 | 312 | 313 | w_x_last = 0.0; 314 | w_y_last = 0.0; 315 | w_z_last = 0.0; 316 | 317 | w_x_rad_s = 0.0; 318 | w_y_rad_s = 0.0; 319 | w_z_rad_s = 0.0; 320 | 321 | w_xBias = 0; 322 | w_yBias = 0; 323 | w_zBias = 0; 324 | 325 | w_xAccumulator = 0.0; 326 | w_yAccumulator = 0.0; 327 | w_zAccumulator = 0.0; 328 | 329 | gyroXLong = 0.0; 330 | gyroYLong = 0.0; 331 | gyroZLong = 0.0; 332 | 333 | gyroRateX = 0.0; 334 | gyroRateY = 0.0; 335 | gyroRateZ = 0.0; 336 | 337 | gyroOrientationX = 0.0; 338 | gyroOrientationY = 0.0; 339 | gyroOrientationZ = 0.0; 340 | } 341 | 342 | void deviceFilter::calibrateGyroscope() 343 | { 344 | // Calibration of Gyro to calculate the Bias 345 | std::cout << "Please use calibrateGyroAndAccel instead!" << std::endl; 346 | } 347 | 348 | void deviceFilter::calibrateGyroAndAccel() 349 | { 350 | 351 | w_xAccumulator = 0.0; 352 | w_yAccumulator = 0.0; 353 | w_zAccumulator = 0.0; 354 | 355 | a_xAccumulator = 0.0; 356 | a_yAccumulator = 0.0; 357 | a_zAccumulator = 0.0; 358 | 359 | w_xBias = 0.0; 360 | w_yBias = 0.0; 361 | w_zBias = 0.0; 362 | 363 | a_xBias = 0.0; 364 | a_yBias = 0.0; 365 | a_zBias = 0.0; 366 | 367 | totalReceivedID66 = 0; 368 | totalReceivedID67 = 0; 369 | totalReceivedID69 = 0; 370 | 371 | totalReceivedFrames = 0; 372 | 373 | bool calibrationDone = FALSE; 374 | 375 | while(!calibrationDone){ 376 | 377 | 378 | if(_usbCANDevice->getReceiveNumber()){ 379 | recNumber = _usbCANDevice->getUnreadFramesNumber(); 380 | } 381 | else{ 382 | _usbCANDevice->showErrorInfo(); 383 | } 384 | 385 | if(_usbCANDevice->receiveData(vco, recNumber)){ 386 | receivedFrames = _usbCANDevice->getReveivedFramsNumber(); 387 | totalReceivedFrames += receivedFrames; 388 | 389 | for(unsigned int i = 0; i < receivedFrames; i++){ 390 | if(vco[i].ID == GYRO_X_Y){ 391 | totalReceivedID66++; 392 | 393 | // Gyro X and Y Rate in Raw format 394 | gyroXLong = vco[i].Data[3] << 24 | vco[i].Data[2] << 16 | vco[i].Data[1] << 8 | vco[i].Data[0]; 395 | gyroYLong = vco[i].Data[7] << 24 | vco[i].Data[6] << 16 | vco[i].Data[5] << 8 | vco[i].Data[4]; 396 | 397 | // Real Gyro X and Y Rate 398 | gyroRateX = gyroXLong * GYRO_RATE_LSB; 399 | gyroRateY = gyroYLong * GYRO_RATE_LSB; 400 | 401 | // Accumulator 402 | w_xAccumulator += gyroRateX; 403 | w_yAccumulator += gyroRateY; 404 | } 405 | else if(vco[i].ID == GYRO_Z_Temp){ 406 | totalReceivedID67++; 407 | 408 | // Gyro Z Rate in Raw format 409 | gyroZLong = vco[i].Data[3] << 24 | vco[i].Data[2] << 16 | vco[i].Data[1] << 8 | vco[i].Data[0]; 410 | tempOfIMULong = vco[i].Data[7]; 411 | 412 | // Real Gyro Z Rate 413 | gyroRateZ = gyroZLong * GYRO_RATE_LSB; 414 | tempIMU = tempOfIMULong * TEMP_IMU_LSB; 415 | 416 | // Accumulator 417 | w_zAccumulator += gyroRateZ; 418 | } 419 | else if(vco[i].ID == ACCEL_X_Y_Z){ 420 | totalReceivedID69++; 421 | 422 | // Accel X, Y, Z Rate in Raw format 423 | accelXLong = vco[i].Data[1] << 8 | vco[i].Data[0]; 424 | accelYLong = vco[i].Data[3] << 8 | vco[i].Data[2]; 425 | accelZLong = vco[i].Data[5] << 8 | vco[i].Data[4]; 426 | 427 | // Real Accel X, Y, Z Rate 428 | accelRateX = accelXLong * ACCEL_RATE_LSB; 429 | accelRateY = accelYLong * ACCEL_RATE_LSB; 430 | accelRateZ = accelZLong * ACCEL_RATE_LSB; 431 | 432 | // Accumulator 433 | a_xAccumulator += accelRateX; 434 | a_yAccumulator += accelRateY; 435 | a_zAccumulator += accelRateZ; 436 | } 437 | else{ 438 | std::cout << "No defined frame ID received!" << std::endl; 439 | } 440 | } 441 | 442 | } 443 | else{ 444 | _usbCANDevice->showErrorInfo(); 445 | } 446 | 447 | // Return the averaged rates 448 | if(totalReceivedFrames >= SAMPLE_FRAMES_FOR_CALIBRATION){ 449 | // Jump out of While Loop 450 | calibrationDone = TRUE; 451 | } 452 | else{ 453 | // Go on sampling 454 | } 455 | 456 | } 457 | 458 | // degree/s 459 | if(totalReceivedID66 != 0){ 460 | w_xBias = w_xAccumulator/totalReceivedID66; 461 | w_yBias = w_yAccumulator/totalReceivedID66; 462 | } 463 | else{ 464 | std::cerr << "receive Frames with ID 0x66 Error!" << std::endl; 465 | } 466 | 467 | if(totalReceivedID67 != 0){ 468 | w_zBias = w_zAccumulator/totalReceivedID67; 469 | } 470 | else{ 471 | std::cerr << "receive Frames with ID 0x67 Error!" << std::endl; 472 | } 473 | 474 | // g 475 | if(totalReceivedID69 != 0){ 476 | a_xBias = a_xAccumulator/totalReceivedID69; 477 | a_yBias = a_yAccumulator/totalReceivedID69; 478 | a_zBias = a_zAccumulator/totalReceivedID69; 479 | } 480 | else{ 481 | std::cerr << "receive Frames with ID 0x69 Error!" << std::endl; 482 | } 483 | 484 | std::cout << "Calibration Done and Zero Offset perspectively: " << endl; 485 | 486 | std::cout << "Gyro X Bias: " << w_xBias << endl; 487 | std::cout << "Gyro Y Bias: " << w_yBias << endl; 488 | std::cout << "Gyro Z Bias: " << w_zBias << endl; 489 | 490 | std::cout << "Accel X Bias: " << a_xBias << endl; 491 | std::cout << "Accel Y Bias: " << a_yBias << endl; 492 | std::cout << "Accel Z Bias: " << a_zBias << endl; 493 | } 494 | 495 | void deviceFilter::setBiasForGyroAndAccel(double gyroXBias, double gyroYBias, double gyroZBias, double accelXBias, double accelYBias, double accelZBias) 496 | { 497 | w_xBias = gyroXBias; 498 | w_yBias = gyroYBias; 499 | w_zBias = gyroZBias; 500 | 501 | a_xBias = accelXBias; 502 | a_yBias = accelYBias; 503 | a_zBias = accelZBias; 504 | } 505 | -------------------------------------------------------------------------------- /devicefilter.h: -------------------------------------------------------------------------------- 1 | #ifndef DEVICEFILTER_H 2 | #define DEVICEFILTER_H 3 | 4 | #include 5 | #include 6 | #include "usbcan.h" 7 | #include "IMUfilter.h" 8 | #include "macrodefs.h" 9 | 10 | using namespace std; 11 | 12 | class deviceFilter : public QObject 13 | { 14 | Q_OBJECT 15 | public: 16 | deviceFilter(); 17 | ~deviceFilter(); 18 | 19 | signals: 20 | void postGyroAndAccelData(double gyroRateX, double gyroRateY, double gyroRateZ, double accelRateX, double accelRateY, double accelRateZ); 21 | void postFilterOrientationData(double pitch, double yaw, double roll); 22 | 23 | public slots: 24 | void filterUpdateAndCalc(); 25 | void sampleGyroAndAccel(); 26 | 27 | public slots: 28 | void testSampleTimer(); 29 | void testFilterTimer(); 30 | 31 | void naiveIntegrationFilter(); 32 | 33 | private: 34 | QTimer* _sampleTimer; 35 | QTimer* _filterTimer; 36 | 37 | USBCAN* _usbCANDevice; 38 | IMUfilter* _imuFilter; 39 | 40 | public: 41 | void attachUsbCANDevice(USBCAN* usbCANDevice); 42 | void attachImuFilter(IMUfilter *imuFilter); 43 | void setBiasForGyroAndAccel(double gyroXBias, double gyroYBias, double gyroZBias, double accelXBias, double accelYBias, double accelZBias); 44 | 45 | public: 46 | 47 | //Offsets for the gyroscope. 48 | //The readings we take when the gyroscope is stationary won't be 0, so we'll 49 | //average a set of readings we do get when the gyroscope is stationary and 50 | //take those away from subsequent readings to ensure the gyroscope is offset 51 | //or "biased" to 0. 52 | double w_xBias; 53 | double w_yBias; 54 | double w_zBias; 55 | 56 | //Offsets for the accelerometer. 57 | //Same as with the gyroscope. 58 | double a_xBias; 59 | double a_yBias; 60 | double a_zBias; 61 | 62 | //Accumulators used for oversampling and then averaging. 63 | volatile double a_xAccumulator; 64 | volatile double a_yAccumulator; 65 | volatile double a_zAccumulator; 66 | volatile double w_xAccumulator; 67 | volatile double w_yAccumulator; 68 | volatile double w_zAccumulator; 69 | 70 | //Accelerometer and gyroscope readings for x, y, z axes. 71 | volatile double a_x; 72 | volatile double a_y; 73 | volatile double a_z; 74 | volatile double w_x; 75 | volatile double w_y; 76 | volatile double w_z; 77 | 78 | // Last reading 79 | volatile double a_x_last; 80 | volatile double a_y_last; 81 | volatile double a_z_last; 82 | volatile double w_x_last; 83 | volatile double w_y_last; 84 | volatile double w_z_last; 85 | 86 | volatile double a_x_m_ss; 87 | volatile double a_y_m_ss; 88 | volatile double a_z_m_ss; 89 | volatile double w_x_rad_s; 90 | volatile double w_y_rad_s; 91 | volatile double w_z_rad_s; 92 | 93 | //Buffer for accelerometer readings. 94 | int readings[3]; 95 | //Number of accelerometer samples we're on. 96 | int accelerometerSamples; 97 | //Number of gyroscope samples we're on. 98 | int gyroscopeSamples; 99 | 100 | // Orientation for X, Y, Z(Pitch, Yaw and Roll) 101 | double gyroOrientationX; 102 | double gyroOrientationY; 103 | double gyroOrientationZ; 104 | 105 | /** 106 | * Prototypes 107 | */ 108 | //Set up the ADXL345 appropriately. 109 | void initializeAcceleromter(void); 110 | 111 | //Calculate the null bias. 112 | void calibrateAccelerometer(void); 113 | 114 | //Take a set of samples and average them. 115 | void sampleAccelerometer(void); 116 | 117 | //Set up the ITG3200 appropriately. 118 | void initializeGyroscope(void); 119 | 120 | //Calculate the null bias. 121 | void calibrateGyroscope(void); 122 | 123 | //Take a set of samples and average them. 124 | void sampleGyroscope(void); 125 | 126 | // Calculate the null bias for both Gyro and Accel 127 | void calibrateGyroAndAccel(void); 128 | 129 | // Take a set of samples both for Gyro and Accel 130 | //void sampleGyroAndAccel(void); 131 | 132 | //Update the filter and calculate the Euler angles. 133 | //void filterUpdateAndCalc(void); 134 | 135 | public: 136 | 137 | // Variables for calculating IMU-F100A5 angles 138 | unsigned long totalReceivedFrames; 139 | unsigned long totalReceivedID66; 140 | unsigned long totalReceivedID67; 141 | unsigned long totalReceivedID69; 142 | 143 | int iterateTimes; 144 | 145 | signed long gyroXLong; 146 | signed long gyroYLong; 147 | signed long gyroZLong; 148 | 149 | signed long tempOfIMULong; 150 | 151 | signed short accelXLong; 152 | signed short accelYLong; 153 | signed short accelZLong; 154 | double gyroRateX; 155 | double gyroRateY; 156 | double gyroRateZ; 157 | 158 | double accelRateX; 159 | double accelRateY; 160 | double accelRateZ; 161 | 162 | double tempIMU; 163 | 164 | public: 165 | 166 | VCI_CAN_OBJ vco[1000]; 167 | DWORD recNumber; 168 | DWORD receivedFrames; 169 | 170 | public: 171 | bool showVerboseInfo; 172 | }; 173 | 174 | #endif // DEVICEFILTER_H 175 | -------------------------------------------------------------------------------- /kerneldlls/CAN232.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hustcalm/OpenIMUFilter/32c89faee3532642f1c7515bf644b8910e4d17d9/kerneldlls/CAN232.dll -------------------------------------------------------------------------------- /kerneldlls/CANETE.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hustcalm/OpenIMUFilter/32c89faee3532642f1c7515bf644b8910e4d17d9/kerneldlls/CANETE.dll -------------------------------------------------------------------------------- /kerneldlls/CANET_TCP.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hustcalm/OpenIMUFilter/32c89faee3532642f1c7515bf644b8910e4d17d9/kerneldlls/CANET_TCP.dll -------------------------------------------------------------------------------- /kerneldlls/PC104C2.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hustcalm/OpenIMUFilter/32c89faee3532642f1c7515bf644b8910e4d17d9/kerneldlls/PC104C2.dll -------------------------------------------------------------------------------- /kerneldlls/PC104CAN.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hustcalm/OpenIMUFilter/32c89faee3532642f1c7515bf644b8910e4d17d9/kerneldlls/PC104CAN.dll -------------------------------------------------------------------------------- /kerneldlls/PCI5121.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hustcalm/OpenIMUFilter/32c89faee3532642f1c7515bf644b8910e4d17d9/kerneldlls/PCI5121.dll -------------------------------------------------------------------------------- /kerneldlls/gisadll.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hustcalm/OpenIMUFilter/32c89faee3532642f1c7515bf644b8910e4d17d9/kerneldlls/gisadll.dll -------------------------------------------------------------------------------- /kerneldlls/gpcidll.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hustcalm/OpenIMUFilter/32c89faee3532642f1c7515bf644b8910e4d17d9/kerneldlls/gpcidll.dll -------------------------------------------------------------------------------- /kerneldlls/isa5420.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hustcalm/OpenIMUFilter/32c89faee3532642f1c7515bf644b8910e4d17d9/kerneldlls/isa5420.dll -------------------------------------------------------------------------------- /kerneldlls/kerneldll.ini: -------------------------------------------------------------------------------- 1 | [KERNELDLL] 2 | COUNT=24 3 | 1=PCI5121.dll 4 | 2=PCI9810B.dll 5 | 3=USBCAN.dll 6 | 4=USBCAN.dll 7 | 5=PCI9820B.dll 8 | 6=CAN232.dll 9 | 7=PCI5121.dll 10 | 8=CANLite.dll 11 | 9=ISA9620B.dll 12 | 10=ISA5420.dll 13 | 11=PC104CAN.dll 14 | 12=CANETE.dll 15 | 13=DNP9810B.dll 16 | 14=PCI9840B.dll 17 | 15=PC104C2.dll 18 | 16=PCI9820I.dll 19 | 17=CANET_TCP.dll 20 | 18=pec9920.dll 21 | 19=pci50xx_u.dll 22 | 20=USBCAN_E.dll 23 | 21=USBCAN_E.dll 24 | 22=pci50xx_u.dll 25 | 23=topcliff_can.dll 26 | 24=pcie9221.dll 27 | -------------------------------------------------------------------------------- /kerneldlls/usbcan.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/hustcalm/OpenIMUFilter/32c89faee3532642f1c7515bf644b8910e4d17d9/kerneldlls/usbcan.dll -------------------------------------------------------------------------------- /macrodefs.h: -------------------------------------------------------------------------------- 1 | #ifndef MACRODEFS_H 2 | #define MACRODEFS_H 3 | 4 | // CAN Frame Micro Defines 5 | #define GYRO_X_Y 0x00000066 6 | #define GYRO_Z_Temp 0x00000067 7 | #define ACCEL_X_Y_Z 0x00000069 8 | 9 | #define GYRO_FULL_32_BIT ((unsigned int)(0xFFFFFFFF)) 10 | #define ACCEL_FULL_16_BIT ((unsigned int)(0xFFFF)) 11 | 12 | #define GYRO_RATE_LSB 0.001 13 | #define ACCEL_RATE_LSB 0.0001 14 | #define TEMP_IMU_LSB 1 15 | 16 | /* Blow macros are imported from http://mbed.org/cookbook/IMU and may not be used 17 | */ 18 | 19 | //Gravity at Earth's surface in m/s/s 20 | #define g0 9.812865328 21 | 22 | //Number of samples to average. 23 | #define SAMPLES 4 24 | 25 | //Number of samples to be averaged for a null bias calculation 26 | //during calibration. 27 | #define CALIBRATION_SAMPLES 128 28 | 29 | //Convert from radians to degrees. 30 | #define toDegrees(x) (x * 57.2957795) 31 | 32 | //Convert from degrees to radians. 33 | #define toRadians(x) (x * 0.01745329252) 34 | 35 | //ITG-3200 sensitivity is 14.375 LSB/(degrees/sec). 36 | #define GYROSCOPE_GAIN (1 / 14.375) 37 | 38 | //Full scale resolution on the ADXL345 is 4mg/LSB. 39 | #define ACCELEROMETER_GAIN (0.004 * g0) 40 | 41 | //Sampling gyroscope at 200Hz. 42 | #define GYRO_RATE 0.005 43 | 44 | //Sampling accelerometer at 200Hz. 45 | #define ACC_RATE 0.005 46 | 47 | //Updating filter at 1/FILTER_RATE Hz. 48 | #define FILTER_RATE 0.01 49 | 50 | // Timer Interval, namely 1000*FILTER_RATE 51 | #define TIMER_INTERVAL 10 52 | 53 | // Sample Frames for Calibration 54 | #define SAMPLE_FRAMES_FOR_CALIBRATION 10000 55 | 56 | #endif // MACRODEFS_H 57 | -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | #include "mainwindow.h" 2 | 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include "devicefilter.h" 10 | 11 | using namespace std; 12 | 13 | // Variables for calculating IMU-F100A5 angles 14 | qint64 totalReceivedFrames = 0; 15 | qint64 totalReceivedID66 = 0; 16 | qint64 totalReceivedID67 = 0; 17 | qint64 totalReceivedID69 = 0; 18 | 19 | int iterateTimes = 100; 20 | 21 | double gyroXAccumulator = 0.0; 22 | double gyroYAccumulator = 0.0; 23 | double gyroZAccumulator = 0.0; 24 | 25 | double gyroXBias = 0.0; 26 | double gyroYBias = 0.0; 27 | double gyroZBias = 0.0; 28 | 29 | double accelXAccumulator = 0.0; 30 | double accelYAccumulator = 0.0; 31 | double accelZAccumulator = 0.0; 32 | 33 | double accelXBias = 0.0; 34 | double accelYBias = 0.0; 35 | double accelZBias = 0.0; 36 | 37 | signed long gyroXLong = 0; 38 | signed long gyroYLong = 0; 39 | signed long gyroZLong = 0; 40 | 41 | signed long tempOfIMULong = 0; 42 | 43 | signed short accelXLong = 0; 44 | signed short accelYLong = 0; 45 | signed short accelZLong = 0; 46 | double gyroRateX = 0; 47 | double gyroRateY = 0; 48 | double gyroRateZ = 0; 49 | 50 | double accelRateX = 0; 51 | double accelRateY = 0; 52 | double accelRateZ = 0; 53 | 54 | double tempIMU = 0; 55 | 56 | IMUfilter *imuFilter; 57 | USBCAN* usbCANDevice; 58 | 59 | int main(int argc, char *argv[]) 60 | { 61 | QApplication a(argc, argv); 62 | MainWindow w; 63 | w.show(); 64 | 65 | cout << "Get Pitch, Yaw, Roll using IMU Filter..." << endl; 66 | 67 | // Open file for log 68 | ofstream outfile("seq_data.txt"); 69 | if(!outfile){ 70 | cerr << "Open Log File Failed!" << endl; 71 | } 72 | else{ 73 | cout << "Log File Ready!" << endl; 74 | } 75 | 76 | // Get USBCAN-I+ to work 77 | usbCANDevice = new USBCAN(USBCAN::_nDefaultDevType, USBCAN::_nDefaultDevIndex, USBCAN::_nDefaultReserved); 78 | 79 | // Open Device 80 | if(usbCANDevice->openDevice()){ 81 | cout << "Device Opened Succesfully!" << endl; 82 | } 83 | else{ 84 | usbCANDevice->showErrorInfo(); 85 | } 86 | 87 | // Initialize Device 88 | PVCI_INIT_CONFIG pInitConfig = new VCI_INIT_CONFIG; 89 | pInitConfig->AccCode = 0; 90 | pInitConfig->AccMask = 0xFFFFFFFF; 91 | pInitConfig->Filter = 1; 92 | pInitConfig->Mode = 0; 93 | pInitConfig->Timing0 = 0x00; 94 | pInitConfig->Timing1 = 0x14; 95 | 96 | if(usbCANDevice->initCAN(pInitConfig)){ 97 | cout << "Device Initialized Successfuly!" << endl; 98 | } 99 | else{ 100 | cout<< "Device Initialized Failed!" << endl; 101 | usbCANDevice->showErrorInfo(); 102 | usbCANDevice->closeDevice(); 103 | } 104 | 105 | // Clear Device buffer first 106 | if(usbCANDevice->clearBuffer()){ 107 | cout << "Device buffer cleard!" << endl; 108 | } 109 | else{ 110 | usbCANDevice->showErrorInfo(); 111 | } 112 | 113 | // Start Communication 114 | VCI_CAN_OBJ vco[1000]; 115 | DWORD recNumber; 116 | DWORD receivedFrames; 117 | 118 | try 119 | { 120 | if(usbCANDevice->startCAN()){ 121 | cout << "Begin to grab Frames..." << endl; 122 | 123 | QElapsedTimer timer; 124 | qint64 nanoSec; 125 | timer.start(); 126 | 127 | while(TRUE && iterateTimes){ 128 | if(usbCANDevice->getReceiveNumber()){ 129 | recNumber = usbCANDevice->getUnreadFramesNumber(); 130 | } 131 | else{ 132 | usbCANDevice->showErrorInfo(); 133 | } 134 | 135 | if(usbCANDevice->receiveData(vco, recNumber)){ 136 | receivedFrames = usbCANDevice->getReveivedFramsNumber(); 137 | totalReceivedFrames += receivedFrames; 138 | 139 | for(unsigned int i = 0; i < receivedFrames; i++){ 140 | if(vco[i].ID == GYRO_X_Y){ 141 | totalReceivedID66++; 142 | 143 | // Gyro X and Y Rate in Raw format 144 | gyroXLong = vco[i].Data[3] << 24 | vco[i].Data[2] << 16 | vco[i].Data[1] << 8 | vco[i].Data[0]; 145 | gyroYLong = vco[i].Data[7] << 24 | vco[i].Data[6] << 16 | vco[i].Data[5] << 8 | vco[i].Data[4]; 146 | 147 | // Real Gyro X and Y Rate 148 | gyroRateX = gyroXLong * GYRO_RATE_LSB; 149 | gyroRateY = gyroYLong * GYRO_RATE_LSB; 150 | 151 | gyroXAccumulator += gyroRateX; 152 | gyroYAccumulator += gyroRateY; 153 | } 154 | else if(vco[i].ID == GYRO_Z_Temp){ 155 | totalReceivedID67++; 156 | 157 | // Gyro Z Rate in Raw format 158 | gyroZLong = vco[i].Data[3] << 24 | vco[i].Data[2] << 16 | vco[i].Data[1] << 8 | vco[i].Data[0]; 159 | tempOfIMULong = vco[i].Data[7]; 160 | 161 | // Real Gyro Z Rate 162 | gyroRateZ = gyroZLong * GYRO_RATE_LSB; 163 | tempIMU = tempOfIMULong * TEMP_IMU_LSB; 164 | 165 | gyroZAccumulator += gyroRateZ; 166 | } 167 | else if(vco[i].ID == ACCEL_X_Y_Z){ 168 | totalReceivedID69++; 169 | 170 | // Accel X, Y, Z Rate in Raw format 171 | accelXLong = vco[i].Data[1] << 8 | vco[i].Data[0]; 172 | accelYLong = vco[i].Data[3] << 8 | vco[i].Data[2]; 173 | accelZLong = vco[i].Data[5] << 8 | vco[i].Data[4]; 174 | 175 | // Real Accel X, Y, Z Rate 176 | accelRateX = accelXLong * ACCEL_RATE_LSB; 177 | accelRateY = accelYLong * ACCEL_RATE_LSB; 178 | accelRateZ = accelZLong * ACCEL_RATE_LSB; 179 | 180 | accelXAccumulator += accelRateX; 181 | accelYAccumulator += accelRateY; 182 | accelZAccumulator += accelRateZ; 183 | } 184 | else{ 185 | cout << "No defined frame ID received!" << endl; 186 | } 187 | } 188 | 189 | // Output Data Every Iteration 190 | FALSE && 191 | cout << "Rates: " << gyroRateX << " " << gyroRateY << " " << gyroRateZ << " " 192 | << accelRateX << " " << accelRateY << " " << accelRateZ << " " 193 | << endl; 194 | } 195 | else{ 196 | usbCANDevice->showErrorInfo(); 197 | } 198 | 199 | if(totalReceivedFrames >= SAMPLE_FRAMES_FOR_CALIBRATION){ 200 | break; 201 | } 202 | else{ 203 | // Go On Sampling 204 | } 205 | } 206 | 207 | nanoSec = timer.nsecsElapsed(); 208 | cout << "Grabbed " << totalReceivedFrames << " in " << nanoSec << " nano seconds!" << endl; 209 | cout << "Grabbed " << totalReceivedID66 << " Frames with ID " << "0x66" << endl; 210 | cout << "Grabbed " << totalReceivedID67 << " Frames with ID " << "0x67" << endl; 211 | cout << "Grabbed " << totalReceivedID69 << " Frames with ID " << "0x69" << endl; 212 | cout << "Frame rates is " << totalReceivedFrames * 1000000000 / nanoSec << endl; 213 | 214 | 215 | 216 | gyroXBias = gyroXAccumulator/totalReceivedID66; 217 | gyroYBias = gyroYAccumulator/totalReceivedID66; 218 | gyroZBias = gyroYAccumulator/totalReceivedID67; 219 | 220 | accelXBias = accelXAccumulator/totalReceivedID69; 221 | accelYBias = accelYAccumulator/totalReceivedID69; 222 | accelZBias = accelYAccumulator/totalReceivedID69; 223 | 224 | std::cout << "Calibration Done and Zero Offset perspectively: " << endl; 225 | 226 | std::cout << "Gyro X Bias: " << gyroXBias << endl; 227 | std::cout << "Gyro Y Bias: " << gyroYBias << endl; 228 | std::cout << "Gyro Z Bias: " << gyroZBias << endl; 229 | 230 | std::cout << "Accel X Bias: " << accelXBias << endl; 231 | std::cout << "Accel Y Bias: " << accelYBias << endl; 232 | std::cout << "Accel Z Bias: " << accelZBias << endl; 233 | } 234 | else{ 235 | usbCANDevice->showErrorInfo(); 236 | } 237 | } 238 | catch(std::exception& e){ 239 | qDebug()<attachUsbCANDevice(usbCANDevice); 249 | imuDeviceFilter->attachImuFilter(imuFilter); 250 | imuDeviceFilter->setBiasForGyroAndAccel(gyroXBias, gyroYBias, gyroZBias, accelXBias, accelYBias, accelZBias); 251 | 252 | QObject::connect(imuDeviceFilter, SIGNAL(postGyroAndAccelData(double,double,double,double,double,double)), &w, SLOT(updateGyroAndAccelData(double,double,double,double,double,double))); 253 | QObject::connect(imuDeviceFilter, SIGNAL(postFilterOrientationData(double,double,double)), &w, SLOT(updateFilterOrientationData(double,double,double))); 254 | 255 | imuDeviceFilter->moveToThread(imuDeviceFilterThread); 256 | imuDeviceFilterThread->start(); 257 | 258 | // Check if we have unread frames 259 | if(usbCANDevice->getReceiveNumber()){ 260 | cout << "Have " << usbCANDevice->getUnreadFramesNumber() << " unread frames left!" << endl; 261 | } 262 | else{ 263 | usbCANDevice->showErrorInfo(); 264 | } 265 | 266 | // Close Device 267 | /* 268 | if(usbCANDevice->closeDevice()){ 269 | cout << "Device Closed!" << endl; 270 | } 271 | else{ 272 | usbCANDevice->showErrorInfo(); 273 | } 274 | */ 275 | 276 | return a.exec(); 277 | } 278 | -------------------------------------------------------------------------------- /mainwindow.cpp: -------------------------------------------------------------------------------- 1 | #include "mainwindow.h" 2 | #include "ui_mainwindow.h" 3 | #include 4 | #include 5 | 6 | MainWindow::MainWindow(QWidget *parent) : 7 | QMainWindow(parent), 8 | ui(new Ui::MainWindow) 9 | { 10 | ui->setupUi(this); 11 | 12 | QTimer* timeUpdater = new QTimer(this); 13 | connect(timeUpdater, SIGNAL(timeout()), this, SLOT(updateCurrentTime())); 14 | timeUpdater->start(1000); 15 | } 16 | 17 | MainWindow::~MainWindow() 18 | { 19 | delete ui; 20 | } 21 | 22 | void MainWindow::updateCurrentTime() 23 | { 24 | QDateTime currentTime = QDateTime::currentDateTime(); 25 | QString currentTimeString = currentTime.toString("yyyy-MM-dd hh:mm:ss dddd"); 26 | ui->label_currentTime->setText(currentTimeString); 27 | } 28 | 29 | void MainWindow::updateGyroAndAccelData(double gyroRateX, double gyroRateY, double gyroRateZ, double accelRateX, double accelRateY, double accelRateZ) 30 | { 31 | ui->lineEdit_Gyro_X->setText(QString::number(gyroRateX)); 32 | ui->lineEdit_Gyro_Y->setText(QString::number(gyroRateY)); 33 | ui->lineEdit_Gyro_Z->setText(QString::number(gyroRateZ)); 34 | 35 | ui->lineEdit_Accel_X->setText(QString::number(accelRateX)); 36 | ui->lineEdit_Accel_Y->setText(QString::number(accelRateY)); 37 | ui->lineEdit_Accel_Z->setText(QString::number(accelRateZ)); 38 | } 39 | 40 | void MainWindow::updateFilterOrientationData(double pitch, double yaw, double roll) 41 | { 42 | ui->lineEdit_Gyro_Orientation_X->setText(QString::number(pitch)); 43 | ui->lineEdit_Gyro_Orientation_Y->setText(QString::number(yaw)); 44 | ui->lineEdit_Gyro_Orientation_Z->setText(QString::number(roll)); 45 | } 46 | -------------------------------------------------------------------------------- /mainwindow.h: -------------------------------------------------------------------------------- 1 | #ifndef MAINWINDOW_H 2 | #define MAINWINDOW_H 3 | 4 | #include 5 | 6 | namespace Ui { 7 | class MainWindow; 8 | } 9 | 10 | class MainWindow : public QMainWindow 11 | { 12 | Q_OBJECT 13 | 14 | public: 15 | explicit MainWindow(QWidget *parent = 0); 16 | ~MainWindow(); 17 | 18 | private: 19 | Ui::MainWindow *ui; 20 | 21 | private slots: 22 | void updateCurrentTime(); 23 | 24 | public slots: 25 | void updateGyroAndAccelData(double gyroRateX, double gyroRateY, double gyroRateZ, double accelRateX, double accelRateY, double accelRateZ); 26 | void updateFilterOrientationData(double pitch, double yaw, double roll); 27 | }; 28 | 29 | #endif // MAINWINDOW_H 30 | -------------------------------------------------------------------------------- /mainwindow.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | MainWindow 4 | 5 | 6 | 7 | 0 8 | 0 9 | 719 10 | 518 11 | 12 | 13 | 14 | MainWindow 15 | 16 | 17 | 18 | 19 | 20 | 40 21 | 30 22 | 46 23 | 13 24 | 25 | 26 | 27 | 加速度计 28 | 29 | 30 | 31 | 32 | 33 | 220 34 | 30 35 | 46 36 | 13 37 | 38 | 39 | 40 | 陀螺仪 41 | 42 | 43 | 44 | 45 | 46 | 220 47 | 160 48 | 46 49 | 13 50 | 51 | 52 | 53 | 角度 54 | 55 | 56 | 57 | 58 | 59 | 40 60 | 160 61 | 46 62 | 13 63 | 64 | 65 | 66 | 位移 67 | 68 | 69 | 70 | 71 | 72 | 20 73 | 180 74 | 16 75 | 71 76 | 77 | 78 | 79 | 80 | 81 | 82 | X 83 | 84 | 85 | 86 | 87 | 88 | 89 | Y 90 | 91 | 92 | 93 | 94 | 95 | 96 | Z 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 190 106 | 181 107 | 16 108 | 71 109 | 110 | 111 | 112 | 113 | 114 | 115 | X 116 | 117 | 118 | 119 | 120 | 121 | 122 | Y 123 | 124 | 125 | 126 | 127 | 128 | 129 | Z 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 208 139 | 181 140 | 135 141 | 74 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 40 160 | 180 161 | 135 162 | 74 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 20 181 | 430 182 | 281 183 | 16 184 | 185 | 186 | 187 | TextLabel 188 | 189 | 190 | 191 | 192 | 193 | 208 194 | 51 195 | 135 196 | 74 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 190 215 | 51 216 | 16 217 | 71 218 | 219 | 220 | 221 | 222 | 223 | 224 | X 225 | 226 | 227 | 228 | 229 | 230 | 231 | Y 232 | 233 | 234 | 235 | 236 | 237 | 238 | Z 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 40 248 | 50 249 | 135 250 | 74 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 20 269 | 50 270 | 16 271 | 71 272 | 273 | 274 | 275 | 276 | 277 | 278 | X 279 | 280 | 281 | 282 | 283 | 284 | 285 | Y 286 | 287 | 288 | 289 | 290 | 291 | 292 | Z 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 0 303 | 0 304 | 719 305 | 21 306 | 307 | 308 | 309 | 310 | 311 | TopToolBarArea 312 | 313 | 314 | false 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | -------------------------------------------------------------------------------- /usbcan.cpp: -------------------------------------------------------------------------------- 1 | #include "usbcan.h" 2 | 3 | USBCAN::USBCAN() 4 | { 5 | // Default constructor, doing nothing 6 | } 7 | 8 | USBCAN::USBCAN(DWORD nDevType, DWORD nDevIndex, DWORD nReserved) 9 | { 10 | // Constructor 11 | //std::cout << "Constructing USBCAN Device..." << std::endl; 12 | 13 | this->_nDevType = nDevType; 14 | this->_nDevIndex = nDevIndex; 15 | this->_nCANIndex = USBCAN::_nDefaultCANIndex; 16 | this->_nReserved = nReserved; 17 | this->_isOpened = FALSE; 18 | this->_isErrorInfoReady = FALSE; 19 | 20 | // Pointer of varies of structures 21 | this->_pInitConfig = NULL; 22 | this->_pBoardInfo = NULL; 23 | this->_pCANStatus = NULL; 24 | this->_pErrorInfo = NULL; 25 | this->_pFilterRecord = NULL; 26 | 27 | // Need memory for Error Report 28 | this->_pErrorInfo = new VCI_ERR_INFO; 29 | 30 | // Pointer to the data buffer 31 | this->_pVCO = NULL; 32 | 33 | //std::cout << "Device construct complete!" << std::endl; 34 | } 35 | 36 | bool USBCAN::openDevice() 37 | { 38 | 39 | if(this->_isOpened){ 40 | return TRUE; 41 | } 42 | else{ 43 | DWORD dwRet; 44 | dwRet = VCI_OpenDevice(this->_nDevType, this->_nDevIndex, this->_nReserved); 45 | this->_retCode = dwRet; 46 | if(dwRet == STATUS_OK){ 47 | this->_isOpened = true; 48 | this->_nCANIndex += 1; 49 | 50 | /* 51 | std::cout << "Device Opened Successfully!" << std::endl; 52 | std::cout << "Device Type: " << this->_nDevType << std::endl; 53 | std::cout << "Device Index: " << this->_nDevIndex << std::endl; 54 | std::cout << "CAN Index: " << this->_nCANIndex<< std::endl; 55 | */ 56 | 57 | return true; 58 | } 59 | else{ 60 | this->readErrorInfo(this->_pErrorInfo); 61 | return FALSE; 62 | } 63 | } 64 | } 65 | 66 | bool USBCAN::initCAN(PVCI_INIT_CONFIG pInitConfig) 67 | { 68 | if(this->_isOpened){ 69 | DWORD dwRet; 70 | this->_pInitConfig = pInitConfig; 71 | std::cout << this->_pInitConfig << std::endl; 72 | std::cout << this->_pInitConfig->AccCode << std::endl; 73 | std::cout << this->_pInitConfig->AccMask << std::endl; 74 | 75 | dwRet = VCI_InitCAN(this->_nDevType, this->_nDevIndex, this->_nCANIndex, this->_pInitConfig); 76 | this->_retCode = dwRet; 77 | if(dwRet == STATUS_OK){ 78 | //std::cout << "Device initialized successfully!" << std::endl; 79 | this->_isInitialized = TRUE; 80 | return TRUE; 81 | } 82 | else{ 83 | this->readErrorInfo(this->_pErrorInfo); 84 | return FALSE; 85 | } 86 | } 87 | else{ 88 | std::cout << "Please open the device before initializing!" << std::endl; 89 | return FALSE; 90 | } 91 | } 92 | 93 | bool USBCAN::startCAN() 94 | { 95 | if(this->_isInitialized){ 96 | DWORD dwRet; 97 | dwRet = VCI_StartCAN(this->_nDevType, this->_nDevIndex, this->_nCANIndex); 98 | this->_retCode = dwRet; 99 | if(dwRet == STATUS_OK){ 100 | this->_isStarted = TRUE; 101 | return TRUE; 102 | } 103 | else{ 104 | this->readErrorInfo(this->_pErrorInfo); 105 | return FALSE; 106 | } 107 | } 108 | else{ 109 | return FALSE; 110 | } 111 | } 112 | 113 | bool USBCAN::getReceiveNumber() 114 | { 115 | if(this->_isInitialized){ 116 | DWORD dwRet; 117 | dwRet = VCI_GetReceiveNum(this->_nDevType, this->_nDevIndex, this->_nCANIndex); 118 | this->_retCode = dwRet; 119 | this->_unReadFramesNumber = dwRet; 120 | return TRUE; 121 | } 122 | else{ 123 | return FALSE; 124 | } 125 | } 126 | 127 | bool USBCAN::receiveData(PVCI_CAN_OBJ pReceive, ULONG len, INT waitTime) 128 | { 129 | if(this->_isStarted){ 130 | DWORD dwRet; 131 | dwRet = VCI_Receive(this->_nDevType, this->_nDevIndex, this->_nCANIndex, pReceive, len, waitTime); 132 | this->_retCode = dwRet; 133 | if(dwRet == 0xFFFFFFFF){ 134 | return FALSE; 135 | } 136 | else{ 137 | this->_receivedFramesNumber = dwRet; 138 | return TRUE; 139 | } 140 | } 141 | else{ 142 | return FALSE; 143 | } 144 | } 145 | 146 | bool USBCAN::clearBuffer() 147 | { 148 | if(this->_isInitialized){ 149 | DWORD dwRet; 150 | dwRet = VCI_ClearBuffer(this->_nDevType, this->_nDevIndex, this->_nCANIndex); 151 | this->_retCode = dwRet; 152 | if(dwRet){ 153 | return TRUE; 154 | } 155 | else{ 156 | return FALSE; 157 | } 158 | } 159 | else{ 160 | return FALSE; 161 | } 162 | } 163 | 164 | bool USBCAN::resetCAN() 165 | { 166 | if(this->_isStarted){ 167 | DWORD dwRet; 168 | dwRet = VCI_ResetCAN(this->_nDevType, this->_nDevIndex, this->_nCANIndex); 169 | this->_retCode = dwRet; 170 | if(dwRet){ 171 | return TRUE; 172 | } 173 | else{ 174 | return FALSE; 175 | } 176 | } 177 | else{ 178 | return FALSE; 179 | } 180 | } 181 | 182 | bool USBCAN::closeDevice() 183 | { 184 | if(this->_isClosed){ 185 | return TRUE; 186 | } 187 | else{ 188 | DWORD dwRet; 189 | dwRet = VCI_CloseDevice(this->_nDevType, this->_nDevIndex); 190 | this->_retCode = dwRet; 191 | if(dwRet){ 192 | this->_isClosed = TRUE; 193 | this->_nDevIndex = USBCAN::_nDefaultCANIndex; 194 | return TRUE; 195 | } 196 | else{ 197 | return FALSE; 198 | } 199 | } 200 | } 201 | 202 | bool USBCAN::readBoardInfo(PVCI_BOARD_INFO pInfo) 203 | { 204 | if(this->_isInitialized){ 205 | DWORD dwRet; 206 | dwRet = VCI_ReadBoardInfo(this->_nDevType, this->_nDevIndex, pInfo); 207 | this->_retCode = dwRet; 208 | if(dwRet){ 209 | return TRUE; 210 | } 211 | else{ 212 | return FALSE; 213 | } 214 | } 215 | else{ 216 | return FALSE; 217 | } 218 | } 219 | 220 | bool USBCAN::readCANStatus(PVCI_CAN_STATUS pCANStatus) 221 | { 222 | if(this->_isInitialized){ 223 | DWORD dwRet; 224 | dwRet = VCI_ReadCANStatus(this->_nDevType, this->_nDevIndex, this->_nCANIndex, pCANStatus); 225 | this->_retCode = dwRet; 226 | if(dwRet){ 227 | return TRUE; 228 | } 229 | else{ 230 | return FALSE; 231 | } 232 | } 233 | else{ 234 | return FALSE; 235 | } 236 | } 237 | 238 | bool USBCAN::getReference(DWORD refType, PVOID pData) 239 | { 240 | if(this->_isInitialized){ 241 | DWORD dwRet; 242 | dwRet = VCI_GetReference(this->_nDevType, this->_nDevIndex, this->_nCANIndex, refType, pData); 243 | this->_retCode = dwRet; 244 | if(dwRet){ 245 | return TRUE; 246 | } 247 | else{ 248 | return FALSE; 249 | } 250 | } 251 | else{ 252 | return FALSE; 253 | } 254 | } 255 | 256 | bool USBCAN::setReference(DWORD refType, PVOID pData) 257 | { 258 | if(this->_isInitialized){ 259 | DWORD dwRet; 260 | dwRet = VCI_SetReference(this->_nDevType, this->_nDevIndex, this->_nCANIndex, refType, pData); 261 | this->_retCode = dwRet; 262 | if(dwRet){ 263 | return TRUE; 264 | } 265 | else{ 266 | return FALSE; 267 | } 268 | } 269 | else{ 270 | return FALSE; 271 | } 272 | } 273 | 274 | bool USBCAN::readErrorInfo(PVCI_ERR_INFO pErrInfo) 275 | { 276 | DWORD dwRet; 277 | dwRet = VCI_ReadErrInfo(this->_nDevType, this->_nDevIndex, this->_nCANIndex, pErrInfo); 278 | this->_retCode = dwRet; 279 | if(dwRet){ 280 | this->_isErrorInfoReady = TRUE; 281 | return TRUE; 282 | } 283 | else{ 284 | this->_isErrorInfoReady = FALSE; 285 | return FALSE; 286 | } 287 | } 288 | 289 | void USBCAN::showErrorInfo() 290 | { 291 | std::cout << "Last return code: " << this->_retCode; 292 | std::cout << " Error code: " << this->_pErrorInfo->ErrCode; 293 | std::cout << std::endl; 294 | } 295 | 296 | void USBCAN::showBoardInfo() 297 | { 298 | // Show basic information about USBCAN-I+ 299 | } 300 | 301 | void USBCAN::showCANStatus() 302 | { 303 | // Show current status of USBCAN-I+ 304 | } 305 | 306 | USBCAN::~USBCAN() 307 | { 308 | if(this->_pVCO){ 309 | delete this->_pVCO; 310 | this->_pVCO = NULL; 311 | } 312 | 313 | if(this->_pBoardInfo){ 314 | delete this->_pBoardInfo; 315 | this->_pBoardInfo = NULL; 316 | } 317 | 318 | if(this->_pCANStatus){ 319 | delete this->_pCANStatus; 320 | this->_pCANStatus = NULL; 321 | } 322 | 323 | if(this->_pErrorInfo){ 324 | delete this->_pErrorInfo; 325 | this->_pErrorInfo = NULL; 326 | } 327 | 328 | if(this->_pFilterRecord){ 329 | delete this->_pFilterRecord; 330 | this->_pFilterRecord = NULL; 331 | } 332 | 333 | if(this->_pInitConfig){ 334 | delete this->_pInitConfig; 335 | this->_pInitConfig = NULL; 336 | } 337 | } 338 | -------------------------------------------------------------------------------- /usbcan.h: -------------------------------------------------------------------------------- 1 | #ifndef USBCAN_H 2 | #define USBCAN_H 3 | 4 | #include 5 | //#include 6 | //#include 7 | #include 8 | #include "ControlCAN.h" 9 | 10 | class USBCAN 11 | { 12 | public: 13 | USBCAN(); 14 | ~USBCAN(); 15 | 16 | USBCAN(DWORD nDevType, DWORD nDevIndex, DWORD nReserved); 17 | 18 | /** 19 | * @brief openDevice 20 | * @param nDevType 21 | * @param nDevIndex 22 | * @param reserved 23 | * @return 24 | */ 25 | bool openDevice(); 26 | 27 | /** 28 | * @brief closeDevice 29 | * @param nDevType 30 | * @param DevIndex 31 | * @return 32 | */ 33 | bool closeDevice(); 34 | 35 | /** 36 | * @brief initCAN 37 | * @param nDevType 38 | * @param nDevIndex 39 | * @param nCANIndex 40 | * @param pInitConfig 41 | * @return 42 | */ 43 | bool initCAN(PVCI_INIT_CONFIG pInitConfig); 44 | 45 | /** 46 | * @brief startCAN 47 | * @param nDevType 48 | * @param nDevIndex 49 | * @param nCANIndex 50 | * @return 51 | */ 52 | bool startCAN(); 53 | 54 | /** 55 | * @brief getReceiveNumber 56 | * @param nDevType 57 | * @param nDevIndex 58 | * @param nCANIndex 59 | * @return 60 | */ 61 | bool getReceiveNumber(); 62 | 63 | /** 64 | * @brief clearBuffer 65 | * @param nDevType 66 | * @param nDevIndex 67 | * @param nCANIndex 68 | * @return 69 | */ 70 | bool clearBuffer(); 71 | 72 | /** 73 | * @brief transmitData 74 | * @param nDevType 75 | * @param nDevIndex 76 | * @param nCANIndex 77 | * @param pSend 78 | * @param len 79 | * @return 80 | */ 81 | bool transmitData(PVCI_CAN_OBJ pSend, ULONG len); 82 | 83 | /** 84 | * @brief transmitData 85 | * @param nDevType 86 | * @param nDevIndex 87 | * @param nCANIndex 88 | * @param pReceive 89 | * @param len 90 | * @param waitTime 91 | * @return 92 | */ 93 | bool receiveData(PVCI_CAN_OBJ pReceive, ULONG len, INT waitTime=-1); 94 | 95 | /** 96 | * @brief resetCAN 97 | * @param nDevType 98 | * @param nDevIndex 99 | * @param nCANIndex 100 | * @return 101 | */ 102 | bool resetCAN(); 103 | 104 | /** 105 | * @brief readBoardInfo 106 | * @param nDevType 107 | * @param nDevIndex 108 | * @param nCANIndex 109 | * @param pInfo 110 | * @return 111 | */ 112 | bool readBoardInfo(PVCI_BOARD_INFO pInfo); 113 | 114 | /** 115 | * @brief readErrorInfo 116 | * @param nDevType 117 | * @param nDevIndex 118 | * @param nCANIndex 119 | * @param pErrInfo 120 | * @return 121 | */ 122 | bool readErrorInfo(PVCI_ERR_INFO pErrInfo); 123 | 124 | /** 125 | * @brief readCANStatus 126 | * @param nDevType 127 | * @param nDevIndex 128 | * @param nCANIndex 129 | * @return 130 | */ 131 | bool readCANStatus(PVCI_CAN_STATUS pCANStatus); 132 | 133 | /** 134 | * @brief getReference 135 | * @param nDevType 136 | * @param nDevIndex 137 | * @param nCANIndex 138 | * @param refType 139 | * @param pData 140 | * @return 141 | */ 142 | bool getReference(DWORD refType, PVOID pData); 143 | 144 | /** 145 | * @brief setReference 146 | * @param nDevType 147 | * @param nDevIndex 148 | * @param nCANIndex 149 | * @param refType 150 | * @param pData 151 | * @return 152 | */ 153 | bool setReference(DWORD refType, PVOID pData); 154 | 155 | /** 156 | * @brief showErrorInfo 157 | */ 158 | void showErrorInfo(); 159 | 160 | /** 161 | * @brief showBoardInfo 162 | */ 163 | void showBoardInfo(); 164 | 165 | /** 166 | * @brief showCANStatus 167 | */ 168 | void showCANStatus(); 169 | 170 | /** 171 | * @brief getUnreadFramesNumber 172 | * @return 173 | */ 174 | ULONG getUnreadFramesNumber(){ 175 | return this->_unReadFramesNumber; 176 | } 177 | 178 | /** 179 | * @brief getTransmittedFramesNumber 180 | * @return 181 | */ 182 | ULONG getTransmittedFramesNumber(){ 183 | return this->_transmittedFramesNumber; 184 | } 185 | 186 | /** 187 | * @brief getReveivedFramsNumber 188 | * @return 189 | */ 190 | ULONG getReveivedFramsNumber(){ 191 | return this->_receivedFramesNumber; 192 | } 193 | 194 | /** 195 | * @brief getLastReturnCode 196 | * @return 197 | */ 198 | DWORD getLastReturnCode(){ 199 | return this->_retCode; 200 | } 201 | 202 | public: 203 | // Device Index 204 | const static DWORD _nDefaultDevType = VCI_USBCAN1; 205 | const static DWORD _nDefaultDevIndex = 0; 206 | const static DWORD _nDefaultCANIndex = -1; 207 | const static DWORD _nDefaultReserved = 0; 208 | 209 | private: 210 | DWORD _nDevType; 211 | DWORD _nDevIndex; 212 | DWORD _nCANIndex; 213 | // Varies of Structures 214 | PVCI_BOARD_INFO _pBoardInfo; 215 | PVCI_INIT_CONFIG _pInitConfig; 216 | PVCI_CAN_STATUS _pCANStatus; 217 | PVCI_ERR_INFO _pErrorInfo; 218 | PVCI_FILTER_RECORD _pFilterRecord; 219 | 220 | // Reference Type for setReference & getReference 221 | DWORD _nRefType; 222 | 223 | //　Buffer for receiving and sending CAN Frames 224 | PVCI_CAN_OBJ _pVCO; 225 | VCI_CAN_OBJ _vco[1000]; 226 | 227 | // Check if the USBCAN device is opened or closed 228 | bool _isOpened; 229 | bool _isClosed; 230 | 231 | // Check if the USBCAN device is initialized 232 | bool _isInitialized; 233 | 234 | // Check if the USBCAN device is started 235 | bool _isStarted; 236 | 237 | // Check if the last read Error Info call is successful 238 | bool _isErrorInfoReady; 239 | 240 | // Number of frames unread in the USBCAN device 241 | ULONG _unReadFramesNumber; 242 | 243 | // Number of frames reveived in one receive call 244 | ULONG _receivedFramesNumber; 245 | 246 | // Number of frames transmitted in one receive call 247 | ULONG _transmittedFramesNumber; 248 | 249 | // Return code of normal API invokes 250 | DWORD _retCode; 251 | 252 | // Reseverd 253 | DWORD _nReserved; 254 | 255 | }; 256 | 257 | #endif // USBCAN_H 258 | --------------------------------------------------------------------------------