├── .gitignore ├── .travis.yml ├── LICENSE ├── Makefile ├── README.md ├── SimpleMotionV2.pri ├── SimpleMotionV2lib.pro ├── appveyor.yml ├── bufferedmotion.c ├── bufferedmotion.h ├── busdevice.c ├── busdevice.h ├── devicedeployment.c ├── devicedeployment.h ├── doc ├── SMV2Protocol.png ├── SMV2Protocol.svg ├── simplemotion protocol subpacket flow diagram.html └── simplemotion protocol subpacket flow diagram.pdf ├── drivers ├── ftdi_d2xx │ ├── sm_d2xx.c │ ├── sm_d2xx.h │ └── third_party │ │ ├── Readme and license.txt │ │ ├── WinTypes.h │ │ ├── ftd2xx.h │ │ ├── linux32 │ │ └── libftd2xx.a │ │ ├── linux64 │ │ └── libftd2xx.a │ │ ├── osx │ │ └── libftd2xx.a │ │ └── win_32bit │ │ └── ftd2xx.lib ├── serial │ ├── pcserialport.c │ └── pcserialport.h └── tcpip │ ├── tcpclient.c │ └── tcpclient.h ├── makefile.nmake ├── simplemotion.c ├── simplemotion.h ├── simplemotion_defs.h ├── simplemotion_private.h ├── simplemotion_types.h ├── sm485.h ├── sm_consts.c ├── tests ├── Makefile └── descriptions.c ├── user_options.h └── utils ├── crc.c └── crc.h /.gitignore: -------------------------------------------------------------------------------- 1 | # Object files 2 | *.o 3 | *.ko 4 | 5 | # Libraries 6 | *.lib 7 | *.a 8 | 9 | # Shared objects (inc. Windows DLLs) 10 | *.dll 11 | *.so 12 | *.so.* 13 | *.dylib 14 | 15 | # Executables 16 | *.exe 17 | *.out 18 | *.app 19 | 20 | # test binaries 21 | tests/descriptions 22 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: c 2 | 3 | script: 4 | # normal library build 5 | - make 6 | # tests build another version of the library (only a static one) and test 7 | # against it with sanitizers 8 | - make test 9 | 10 | matrix: 11 | include: 12 | - compiler: gcc 13 | os: linux 14 | 15 | - compiler: clang 16 | os: osx 17 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | Apache License 3 | Version 2.0, January 2004 4 | http://www.apache.org/licenses/ 5 | 6 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 7 | 8 | 1. Definitions. 9 | 10 | "License" shall mean the terms and conditions for use, reproduction, 11 | and distribution as defined by Sections 1 through 9 of this document. 12 | 13 | "Licensor" shall mean the copyright owner or entity authorized by 14 | the copyright owner that is granting the License. 15 | 16 | "Legal Entity" shall mean the union of the acting entity and all 17 | other entities that control, are controlled by, or are under common 18 | control with that entity. For the purposes of this definition, 19 | "control" means (i) the power, direct or indirect, to cause the 20 | direction or management of such entity, whether by contract or 21 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 22 | outstanding shares, or (iii) beneficial ownership of such entity. 23 | 24 | "You" (or "Your") shall mean an individual or Legal Entity 25 | exercising permissions granted by this License. 26 | 27 | "Source" form shall mean the preferred form for making modifications, 28 | including but not limited to software source code, documentation 29 | source, and configuration files. 30 | 31 | "Object" form shall mean any form resulting from mechanical 32 | transformation or translation of a Source form, including but 33 | not limited to compiled object code, generated documentation, 34 | and conversions to other media types. 35 | 36 | "Work" shall mean the work of authorship, whether in Source or 37 | Object form, made available under the License, as indicated by a 38 | copyright notice that is included in or attached to the work 39 | (an example is provided in the Appendix below). 40 | 41 | "Derivative Works" shall mean any work, whether in Source or Object 42 | form, that is based on (or derived from) the Work and for which the 43 | editorial revisions, annotations, elaborations, or other modifications 44 | represent, as a whole, an original work of authorship. For the purposes 45 | of this License, Derivative Works shall not include works that remain 46 | separable from, or merely link (or bind by name) to the interfaces of, 47 | the Work and Derivative Works thereof. 48 | 49 | "Contribution" shall mean any work of authorship, including 50 | the original version of the Work and any modifications or additions 51 | to that Work or Derivative Works thereof, that is intentionally 52 | submitted to Licensor for inclusion in the Work by the copyright owner 53 | or by an individual or Legal Entity authorized to submit on behalf of 54 | the copyright owner. For the purposes of this definition, "submitted" 55 | means any form of electronic, verbal, or written communication sent 56 | to the Licensor or its representatives, including but not limited to 57 | communication on electronic mailing lists, source code control systems, 58 | and issue tracking systems that are managed by, or on behalf of, the 59 | Licensor for the purpose of discussing and improving the Work, but 60 | excluding communication that is conspicuously marked or otherwise 61 | designated in writing by the copyright owner as "Not a Contribution." 62 | 63 | "Contributor" shall mean Licensor and any individual or Legal Entity 64 | on behalf of whom a Contribution has been received by Licensor and 65 | subsequently incorporated within the Work. 66 | 67 | 2. Grant of Copyright License. Subject to the terms and conditions of 68 | this License, each Contributor hereby grants to You a perpetual, 69 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 70 | copyright license to reproduce, prepare Derivative Works of, 71 | publicly display, publicly perform, sublicense, and distribute the 72 | Work and such Derivative Works in Source or Object form. 73 | 74 | 3. Grant of Patent License. Subject to the terms and conditions of 75 | this License, each Contributor hereby grants to You a perpetual, 76 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 77 | (except as stated in this section) patent license to make, have made, 78 | use, offer to sell, sell, import, and otherwise transfer the Work, 79 | where such license applies only to those patent claims licensable 80 | by such Contributor that are necessarily infringed by their 81 | Contribution(s) alone or by combination of their Contribution(s) 82 | with the Work to which such Contribution(s) was submitted. If You 83 | institute patent litigation against any entity (including a 84 | cross-claim or counterclaim in a lawsuit) alleging that the Work 85 | or a Contribution incorporated within the Work constitutes direct 86 | or contributory patent infringement, then any patent licenses 87 | granted to You under this License for that Work shall terminate 88 | as of the date such litigation is filed. 89 | 90 | 4. Redistribution. You may reproduce and distribute copies of the 91 | Work or Derivative Works thereof in any medium, with or without 92 | modifications, and in Source or Object form, provided that You 93 | meet the following conditions: 94 | 95 | (a) You must give any other recipients of the Work or 96 | Derivative Works a copy of this License; and 97 | 98 | (b) You must cause any modified files to carry prominent notices 99 | stating that You changed the files; and 100 | 101 | (c) You must retain, in the Source form of any Derivative Works 102 | that You distribute, all copyright, patent, trademark, and 103 | attribution notices from the Source form of the Work, 104 | excluding those notices that do not pertain to any part of 105 | the Derivative Works; and 106 | 107 | (d) If the Work includes a "NOTICE" text file as part of its 108 | distribution, then any Derivative Works that You distribute must 109 | include a readable copy of the attribution notices contained 110 | within such NOTICE file, excluding those notices that do not 111 | pertain to any part of the Derivative Works, in at least one 112 | of the following places: within a NOTICE text file distributed 113 | as part of the Derivative Works; within the Source form or 114 | documentation, if provided along with the Derivative Works; or, 115 | within a display generated by the Derivative Works, if and 116 | wherever such third-party notices normally appear. The contents 117 | of the NOTICE file are for informational purposes only and 118 | do not modify the License. You may add Your own attribution 119 | notices within Derivative Works that You distribute, alongside 120 | or as an addendum to the NOTICE text from the Work, provided 121 | that such additional attribution notices cannot be construed 122 | as modifying the License. 123 | 124 | You may add Your own copyright statement to Your modifications and 125 | may provide additional or different license terms and conditions 126 | for use, reproduction, or distribution of Your modifications, or 127 | for any such Derivative Works as a whole, provided Your use, 128 | reproduction, and distribution of the Work otherwise complies with 129 | the conditions stated in this License. 130 | 131 | 5. Submission of Contributions. Unless You explicitly state otherwise, 132 | any Contribution intentionally submitted for inclusion in the Work 133 | by You to the Licensor shall be under the terms and conditions of 134 | this License, without any additional terms or conditions. 135 | Notwithstanding the above, nothing herein shall supersede or modify 136 | the terms of any separate license agreement you may have executed 137 | with Licensor regarding such Contributions. 138 | 139 | 6. Trademarks. This License does not grant permission to use the trade 140 | names, trademarks, service marks, or product names of the Licensor, 141 | except as required for reasonable and customary use in describing the 142 | origin of the Work and reproducing the content of the NOTICE file. 143 | 144 | 7. Disclaimer of Warranty. Unless required by applicable law or 145 | agreed to in writing, Licensor provides the Work (and each 146 | Contributor provides its Contributions) on an "AS IS" BASIS, 147 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 148 | implied, including, without limitation, any warranties or conditions 149 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 150 | PARTICULAR PURPOSE. You are solely responsible for determining the 151 | appropriateness of using or redistributing the Work and assume any 152 | risks associated with Your exercise of permissions under this License. 153 | 154 | 8. Limitation of Liability. In no event and under no legal theory, 155 | whether in tort (including negligence), contract, or otherwise, 156 | unless required by applicable law (such as deliberate and grossly 157 | negligent acts) or agreed to in writing, shall any Contributor be 158 | liable to You for damages, including any direct, indirect, special, 159 | incidental, or consequential damages of any character arising as a 160 | result of this License or out of the use or inability to use the 161 | Work (including but not limited to damages for loss of goodwill, 162 | work stoppage, computer failure or malfunction, or any and all 163 | other commercial damages or losses), even if such Contributor 164 | has been advised of the possibility of such damages. 165 | 166 | 9. Accepting Warranty or Additional Liability. While redistributing 167 | the Work or Derivative Works thereof, You may choose to offer, 168 | and charge a fee for, acceptance of support, warranty, indemnity, 169 | or other liability obligations and/or rights consistent with this 170 | License. However, in accepting such obligations, You may act only 171 | on Your own behalf and on Your sole responsibility, not on behalf 172 | of any other Contributor, and only if You agree to indemnify, 173 | defend, and hold each Contributor harmless for any liability 174 | incurred by, or claims asserted against, such Contributor by reason 175 | of your accepting any such warranty or additional liability. 176 | 177 | END OF TERMS AND CONDITIONS 178 | 179 | APPENDIX: How to apply the Apache License to your work. 180 | 181 | To apply the Apache License to your work, attach the following 182 | boilerplate notice, with the fields enclosed by brackets "[]" 183 | replaced with your own identifying information. (Don't include 184 | the brackets!) The text should be enclosed in the appropriate 185 | comment syntax for the file format. We also recommend that a 186 | file or class name and description of purpose be included on the 187 | same "printed page" as the copyright notice for easier 188 | identification within third-party archives. 189 | 190 | Copyright [yyyy] [name of copyright owner] 191 | 192 | Licensed under the Apache License, Version 2.0 (the "License"); 193 | you may not use this file except in compliance with the License. 194 | You may obtain a copy of the License at 195 | 196 | http://www.apache.org/licenses/LICENSE-2.0 197 | 198 | Unless required by applicable law or agreed to in writing, software 199 | distributed under the License is distributed on an "AS IS" BASIS, 200 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 201 | See the License for the specific language governing permissions and 202 | limitations under the License. 203 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | # This is a Makefile for Travis CI, not tested for other purposes 2 | 3 | SOURCES = $(wildcard *.c) \ 4 | drivers/serial/pcserialport.c \ 5 | drivers/tcpip/tcpclient.c 6 | 7 | OBJECTS = $(SOURCES:%.c=%.o) 8 | 9 | #user optionsare the ones starting with -D below 10 | #be sure to check also user_options.h for more 11 | CPPFLAGS = -I. -Iutils/ -DENABLE_BUILT_IN_DRIVERS 12 | CFLAGS = -Wall -Wextra -DENABLE_BUILT_IN_DRIVERS -Iutils/ 13 | 14 | all: libsimplemotionv2.a 15 | 16 | libsimplemotionv2.a: $(OBJECTS) 17 | ar rcs $@ $^ 18 | 19 | test: 20 | # this target executes the test cases; the test build depends on 21 | # gcc/clag sanitizers and those are not available except on unix alike 22 | # platforms/targets 23 | make -C tests 24 | 25 | .PHONY: clean 26 | clean: 27 | rm -f $(OBJECTS) 28 | make -C tests clean 29 | 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![Linux Build Status](https://travis-ci.org/GraniteDevices/SimpleMotionV2.svg?branch=master)](https://travis-ci.org/GraniteDevices/SimpleMotionV2) 2 | [![Windows Build Status](https://ci.appveyor.com/api/projects/status/github/GraniteDevices/SimpleMotionV2)](https://ci.appveyor.com/project/TeroK/simplemotionv2) 3 | 4 | SimpleMotionV2 5 | ============== 6 | 7 | This is a SimpleMotion V2 library, which is an API to control motor controller from any programmable platform, such as PC (Linux, Win, Mac), Rasperry Pi, MCU or PLC system. 8 | 9 | For main documentation, see: 10 | http://granitedevices.com/wiki/SimpleMotion_V2 11 | 12 | Files & usage 13 | ============= 14 | 15 | There are files that are needed for basic SimpleMotion usage, and files that are optional and needed only for certain features. Following may be useful info when porting library to different platforms (such as embedded MCU) to avoid unnecessary work on some files. 16 | 17 | Compulsory 18 | ---------- 19 | 20 | Following files are the core library. These must be included in the compiled projects to achieve a working solution. 21 | 22 | - simplemotion.c/.h 23 | - sm485.h 24 | - sm_consts.c 25 | - simplemotion_defs.h 26 | - simplemotion_private.h 27 | - busdevice.c/.h 28 | 29 | Feature specific 30 | ---------------- 31 | 32 | Following files are not required in the core library, however you might need them depending on your application. 33 | 34 | - bufferedmotion.c/.h - Library used for buffered motion stream applications 35 | - devicedeployment.c/.h - Library used for installing firmware and loading settings into target devices. Can be used to configure devices automatically in-system. 36 | - files in drivers/ folder - These are platofrm & hardware specific SM bus interface device drivers 37 | 38 | Porting to new platform 39 | ----------------------- 40 | 41 | Following files need modification if ported to another platform where SimpleMotion communication interface device driver does not yet exist 42 | 43 | - If porting to a non-PC system, make sure that ENABLE_BUILT_IN_DRIVERS is not defined at compilation time (see qmake .pri file for clues) and use smOpenBusWithCallbacks instead of smOpenBus 44 | - Write custom communication interface device driver and use smOpenBusWithCallbacks to open bus with custom driver callbacks 45 | - See existing drivers for example 46 | - Only four simple functions are needed to write custom port driver: open port, close port, write port and read port 47 | 48 | Using SimpleMotion 49 | ================== 50 | 51 | ## In Qt Application 52 | 53 | To include SM library in Qt project, simply place the library files under project folder and add the following line in your .pro file: 54 | 55 | include(SimpleMotionV2/SimpleMotionV2.pri) 56 | 57 | ## Creating shared / dynamic library 58 | 59 | It is possible to compile shared and dynamic libraries (i.e. .dll or .a file) from the SM library source package. One of the easiest ways of compiling library is to use Qt Creator, where a ready-to-compile project file is provided. Just open SimpleMotionV2lib.pro in Qt Creator and compile it with the compiler of your choice. The resulting library files may be used in other applications. 60 | 61 | Alternatively create a new library project in your favorite programming tool and compile the provided source codes into a libraray. You might need to study workings of the .pri file to succeed. 62 | 63 | Windows users notice that a .dll library compiled with MinGW might not be compatible with MSVC. So if you're developing with MSVC, it's best to complile the dll using MSVC compiler. Qt Creator allows using Visual Studio's MSVC as compiler when configured properly. 64 | 65 | ## In Python, C# & others 66 | 67 | Most of programming languages have wrapper solutions that can call C functions. To get started, search some examples from the Internet for your programmign language. 68 | 69 | If you write a wrapper library or any form of working demo application to your non-C programming language, feel free to contribute it to us and get credited! 70 | 71 | ## In Visual Studio C/C++ 72 | 73 | If you wish to link library statically in other programming environments, i.e. in Visual Studio, it might be easiest to grap the sources and add them into the project so that they will be compiled along your project. This way library will be included in your application and it does not depend on external shared or dynamic libraries. 74 | 75 | ## In LabView 76 | 77 | LabView allows including .dll file in the project and calling the C functions. To do that, first compile a .dll file as instructed in "Creating shared / dynamic library" section above. 78 | 79 | ## In bare metal (embedded MCU) 80 | 81 | SimpleMotion library has been written to compile and run also on microcontrollers. On peripheral side it requires basically just USART and RS485 driver that can produce at least the default 468 000 BPS baud rate with less than 5% error. 82 | 83 | In practive one just has to write a custom USART/RS485 driver in SM library to get going. It's beneficial to use DMA with USART to avoid missing bytes. 84 | 85 | To get started, see existing serial port drivers unders `drivers/serial` subfolder and add your own. After that, one can use `smBDOpenWithCallbacks` or modify `busdevice.c` to support the new driver. 86 | 87 | Examples 88 | ======== 89 | 90 | For practical usage examples, refer to https://github.com/GraniteDevices/SimpleMotionV2Examples 91 | 92 | About different branches 93 | ======================== 94 | 95 | There are multiple branches in this repository. Summary: 96 | 97 | * **master:** Latest stable version. The goal here is to keep API stable and backwards compatible with earlier master versions. Master branch will be updated occasionally by merge commit from develop. 98 | * **develop:** Latest development version, may contain unfinished features & changing API. Be warned that your application using under development features might break. 99 | * **feature/nnn:** Under development features that eventually shall be merged to develop. 100 | 101 | Usually it's safest to use master version. If latest additions are necessary, then use develop. 102 | -------------------------------------------------------------------------------- /SimpleMotionV2.pri: -------------------------------------------------------------------------------- 1 | #User options 2 | #be sure to check also user_options.h for more 3 | 4 | #if 0, then compile bare SM library without comm interface device drivers (user should then open bus with smOpenBusWithCallbacks and write custom port driver callbacks) 5 | INCLUDE_BUILT_IN_DRIVERS = 1 6 | 7 | #requires FTDI D2XX driver & library. benefit of this support is automatic detection of correct device and automatic low latency setting for FTDI USB serial converters 8 | SUPPORT_FTDI_D2XX_DRIVER = 1 9 | 10 | INCLUDEPATH += $$PWD $$PWD/utils 11 | DEPENDPATH += $$PWD 12 | 13 | 14 | DEFINES += SIMPLEMOTIONV2_LIBRARY 15 | 16 | #define ENABLE_DEBUG_PRINTS to enable SM debug printing (enabling it may slow down & grow binary significantly especially on MCU systems) 17 | #DEFINES += ENABLE_DEBUG_PRINTS 18 | 19 | SOURCES += $$PWD/sm_consts.c $$PWD/simplemotion.c $$PWD/busdevice.c \ 20 | $$PWD/bufferedmotion.c $$PWD/devicedeployment.c $$PWD/utils/crc.c 21 | 22 | HEADERS += $$PWD/simplemotion_private.h\ 23 | $$PWD/busdevice.h $$PWD/simplemotion.h $$PWD/sm485.h $$PWD/simplemotion_defs.h \ 24 | $$PWD/bufferedmotion.h $$PWD/devicedeployment.h \ 25 | $$PWD/user_options.h \ 26 | $$PWD/simplemotion_types.h \ 27 | $$PWD/user_options.h $$PWD/utils/crc.h 28 | 29 | 30 | greaterThan(INCLUDE_BUILT_IN_DRIVERS, 0+) { 31 | SOURCES += $$PWD/drivers/serial/pcserialport.c $$PWD/drivers/tcpip/tcpclient.c 32 | HEADERS += $$PWD/drivers/serial/pcserialport.h $$PWD/drivers/tcpip/tcpclient.h 33 | DEFINES += ENABLE_BUILT_IN_DRIVERS 34 | win32 { 35 | LIBS+=-lws2_32 #needed for tcp ip API 36 | } 37 | 38 | #If FTDI D2XX support is enabled 39 | greaterThan(SUPPORT_FTDI_D2XX_DRIVER, 0+) { 40 | SOURCES += $$PWD/drivers/ftdi_d2xx/sm_d2xx.c 41 | HEADERS += $$PWD/drivers/ftdi_d2xx/sm_d2xx.c 42 | macx:LIBS += $$PWD/drivers/ftdi_d2xx/third_party/osx/libftd2xx.a -framework CoreFoundation #mac will needs insetalling some FTDI helper tool & reboot to make port open to work. see d2xx downloads page from ftdi. 43 | win32:LIBS += $$PWD/drivers/ftdi_d2xx/third_party/win_32bit/ftd2xx.lib 44 | unix|linux{ 45 | equals(QMAKE_HOST.arch,"x86_64"){ 46 | LIBS += $$PWD/drivers/ftdi_d2xx/third_party/linux64/libftd2xx.a 47 | } else { 48 | LIBS += $$PWD/drivers/ftdi_d2xx/third_party/linux32/libftd2xx.a 49 | } 50 | } 51 | DEFINES += FTDI_D2XX_SUPPORT 52 | } 53 | } 54 | 55 | -------------------------------------------------------------------------------- /SimpleMotionV2lib.pro: -------------------------------------------------------------------------------- 1 | #------------------------------------------------- 2 | # 3 | # qmake project to compile static & dymaic libraries of SimpleMotion 4 | 5 | # In windows, MinGW will output .a and .dll, 6 | # MSVC2010 will ouput .lib and .dll. 7 | # In linux, MinGW will output .so, .so.1, .so.1.0 and .so.1.0.0 - .lib, .a and .so are import libraries. They help link your code to the library and is needed when you build your file(.a files not all the time). 8 | # 9 | #------------------------------------------------- 10 | 11 | include(SimpleMotionV2.pri) 12 | 13 | QT -= core gui 14 | 15 | TARGET = simplemotionv2 16 | TEMPLATE = lib 17 | 18 | -------------------------------------------------------------------------------- /appveyor.yml: -------------------------------------------------------------------------------- 1 | image: Visual Studio 2015 2 | 3 | platform: 4 | - x86 5 | - x64 6 | 7 | build_script: 8 | - if "%platform%" == "x86" call "%VS140COMNTOOLS%\..\..\VC\vcvarsall.bat" x86 9 | - if "%platform%" == "x64" call "%VS140COMNTOOLS%\..\..\VC\vcvarsall.bat" amd64 10 | - nmake /f makefile.nmake 11 | -------------------------------------------------------------------------------- /bufferedmotion.c: -------------------------------------------------------------------------------- 1 | #include "simplemotion.h" 2 | #include "user_options.h" 3 | #include "simplemotion_private.h" 4 | #include "bufferedmotion.h" 5 | #include "sm485.h" 6 | 7 | /** initialize buffered motion for one axis with address and samplerate (Hz) */ 8 | SM_STATUS smBufferedInit(BufferedMotionAxis *newAxis, smbus handle, smaddr deviceAddress, smint32 sampleRate, smint16 readParamAddr, smuint8 readDataLength ) 9 | { 10 | //value out of range 11 | if(sampleRate<1 || sampleRate>2500) 12 | return recordStatus(handle,SM_ERR_PARAMETER); 13 | 14 | newAxis->initialized=smfalse; 15 | newAxis->bushandle=handle; 16 | newAxis->samplerate=sampleRate; 17 | newAxis->deviceAddress=deviceAddress; 18 | newAxis->readParamAddr=readParamAddr; 19 | newAxis->readParamLength=readDataLength; 20 | newAxis->readParamInitialized=smfalse; 21 | newAxis->numberOfDiscardableReturnDataPackets=0; 22 | newAxis->driveClock=0; 23 | newAxis->bufferFill=0; 24 | newAxis->numberOfPendingReadPackets=0; 25 | newAxis->driveFlagsModifiedAtInit=smfalse; 26 | newAxis->deviceCapabilityFlags1=0; 27 | newAxis->deviceCapabilityFlags2=0; 28 | 29 | //discard any existing data in buffer, and to get correct reading of device buffer size 30 | smSetParameter( newAxis->bushandle, newAxis->deviceAddress, SMP_SYSTEM_CONTROL,SMP_SYSTEM_CONTROL_ABORTBUFFERED); 31 | 32 | //after abort, we can read the maximum size of data in device buffer 33 | smRead2Parameters(newAxis->bushandle,newAxis->deviceAddress,SMP_BUFFER_FREE_BYTES,&newAxis->bufferLength,SMP_SM_VERSION,&newAxis->smProtocolVersion); 34 | newAxis->bufferFreeBytes=newAxis->bufferLength; 35 | 36 | if(smRead1Parameter(handle,deviceAddress,SMP_DRIVE_FLAGS,&newAxis->driveFlagsBeforeInit)!=SM_OK) 37 | return getCumulativeStatus(handle);//if error happens in read, avoid altering the flag (later) 38 | 39 | if(newAxis->smProtocolVersion>=28)//V28 and later support device capability flags, so read them 40 | { 41 | smRead2Parameters(newAxis->bushandle,newAxis->deviceAddress,SMP_DEVICE_CAPABILITIES1,&newAxis->deviceCapabilityFlags1,SMP_DEVICE_CAPABILITIES2,&newAxis->deviceCapabilityFlags2); 42 | } 43 | 44 | //set linear interpolation mode if supported 45 | if(newAxis->deviceCapabilityFlags1&DEVICE_CAPABILITY1_BUFFERED_MOTION_LINEAR_INTERPOLATION) 46 | { 47 | smSetParameter(handle,deviceAddress,SMP_BUFFERED_MODE,BUFFERED_INTERPOLATION_MODE_LINEAR);//enable interpolation of buffered setpoints 48 | } 49 | else//use traditional nearest neighbor setpoint mode 50 | { 51 | //set input smoothing filter on [CIS] if samplerate is not maximum. with filter samplerates 250,500,750,1000,1250 etc run smooth. needed only for old nearest neighbor setpoint "interpolation" mode 52 | if(sampleRate<2500) 53 | { 54 | smSetParameter(handle,deviceAddress,SMP_DRIVE_FLAGS,newAxis->driveFlagsBeforeInit|FLAG_USE_INPUT_LP_FILTER); 55 | newAxis->driveFlagsModifiedAtInit=smtrue; 56 | } 57 | } 58 | 59 | //set buffer execution rate to max so init commands go fast 60 | smSetParameter(handle,deviceAddress,SMP_BUFFERED_CMD_PERIOD,10000/2500); 61 | 62 | //set acceleration to "infinite" to avoid modification of user supplied trajectory inside drive 63 | smRead1Parameter(handle,deviceAddress,SMP_TRAJ_PLANNER_ACCEL,&newAxis->driveAccelerationBeforeInit);//store original for restoration 64 | smSetParameter(handle,deviceAddress,SMP_TRAJ_PLANNER_ACCEL,32767); 65 | 66 | //set buffer execution rate 67 | smSetParameter(handle,deviceAddress,SMP_BUFFERED_CMD_PERIOD,10000/sampleRate); 68 | 69 | //FIXME can cause unnecessary initialized=false status if there was error flags in cumulative status before calling this func 70 | if(getCumulativeStatus(handle)==SM_OK) 71 | newAxis->initialized=smtrue; 72 | 73 | return getCumulativeStatus(handle); 74 | } 75 | 76 | /** uninitialize axis from buffered motion, recommended to call this before closing bus so drive's adjusted parameters are restored to originals*/ 77 | SM_STATUS smBufferedDeinit( BufferedMotionAxis *axis ) 78 | { 79 | smBufferedAbort(axis); 80 | 81 | //restore drive in pre-init state 82 | if(axis->initialized==smtrue) 83 | { 84 | smSetParameter(axis->bushandle,axis->deviceAddress,SMP_TRAJ_PLANNER_ACCEL,axis->driveAccelerationBeforeInit); 85 | if(axis->driveFlagsModifiedAtInit==smtrue)//if flags parameter modified, then restore the origianl value 86 | smSetParameter(axis->bushandle,axis->deviceAddress,SMP_DRIVE_FLAGS,axis->driveFlagsBeforeInit); 87 | } 88 | 89 | axis->initialized=smfalse; 90 | axis->readParamInitialized=smfalse; 91 | 92 | return getCumulativeStatus(axis->bushandle); 93 | } 94 | 95 | 96 | /* this also starts buffered motion when it's not running*/ 97 | SM_STATUS smBufferedRunAndSyncClocks(BufferedMotionAxis *axis) 98 | { 99 | return smGetBufferClock( axis->bushandle, axis->deviceAddress, &axis->driveClock ); 100 | } 101 | 102 | SM_STATUS smBufferedGetFree(BufferedMotionAxis *axis, smint32 *numBytesFree ) 103 | { 104 | smint32 freebytes; 105 | 106 | if(smRead1Parameter(axis->bushandle,axis->deviceAddress,SMP_BUFFER_FREE_BYTES,&freebytes)!=SM_OK) 107 | { 108 | *numBytesFree=0;//read has failed, assume 0 109 | return getCumulativeStatus(axis->bushandle); 110 | } 111 | 112 | axis->bufferFreeBytes=freebytes; 113 | axis->bufferFill=100*(axis->bufferLength-freebytes)/axis->bufferLength;//calc buffer fill 0-100% 114 | 115 | *numBytesFree=freebytes; 116 | 117 | return getCumulativeStatus(axis->bushandle); 118 | } 119 | 120 | smint32 smBufferedGetMaxFillSize(BufferedMotionAxis *axis, smint32 numBytesFree ) 121 | { 122 | //even if we have lots of free space in buffer, we can only send up to SM485_MAX_PAYLOAD_BYTES bytes at once in one SM transmission 123 | if(numBytesFree>SM485_MAX_PAYLOAD_BYTES) 124 | numBytesFree=SM485_MAX_PAYLOAD_BYTES; 125 | 126 | //calculate number of points that can be uploaded to buffer (max size SM485_MAX_PAYLOAD_BYTES bytes and fill consumption is 2+4+2+3*(n-1) bytes) 127 | if(axis->readParamInitialized==smtrue) 128 | //*numPoints=(freebytes-2-4-2)/3+1; 129 | return numBytesFree/4; 130 | else 131 | //*numPoints=(freebytes-2-4-2 -2-2-2-2)/3+1;//if read data uninitialized, it takes extra 8 bytes to init on next fill, so reduce it here 132 | return (numBytesFree-2-3-2-3-2)/4;//if read data uninitialized, it takes extra n bytes to init on next fill, so reduce it here 133 | } 134 | 135 | smint32 smBufferedGetBytesConsumed(BufferedMotionAxis *axis, smint32 numFillPoints ) 136 | { 137 | //calculate number of bytes that the number of fill points will consume from buffer 138 | if(axis->readParamInitialized==smtrue) 139 | return numFillPoints*4; 140 | else 141 | return numFillPoints*4 +2+3+2+3+2;//if read data uninitialized, it takes extra n bytes to init on next fill, so reduce it here 142 | } 143 | 144 | 145 | SM_STATUS smBufferedFillAndReceive(BufferedMotionAxis *axis, smint32 numFillPoints, smint32 *fillPoints, smint32 *numReceivedPoints, smint32 *receivedPoints, smint32 *bytesFilled ) 146 | { 147 | smint32 bytesUsed=0; 148 | 149 | //if(freeBytesInDeviceBuffer>=cmdBufferSizeBytes) 150 | // emit message(Warning,"Buffer underrun on axis "+QString::number(ax)); 151 | 152 | //freeBytesInDeviceBuffer-=8; 153 | // if(drives[ax].bufferedStreamInitialized==false) 154 | // cmdBufferSizeBytes=freeBytesInDeviceBuffer;//get empty buffer size 155 | 156 | //first initialize the stream if not done yet 157 | if(axis->readParamInitialized==smfalse) 158 | { 159 | //set acceleration to "infinite" to avoid modification of user supplied trajectory inside drive 160 | smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_RETURN_PARAM_ADDR); 161 | smAppendSMCommandToQueue(axis->bushandle,SM_WRITE_VALUE_24B,axis->readParamAddr); 162 | smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_RETURN_PARAM_LEN); 163 | smAppendSMCommandToQueue(axis->bushandle,SM_WRITE_VALUE_24B,axis->readParamLength); 164 | smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_ABSOLUTE_SETPOINT); 165 | bytesUsed+=2+3+2+3+2; 166 | 167 | //next time we read return data, we discard first 4 return packets to avoid unexpected read data to user 168 | axis->numberOfDiscardableReturnDataPackets+=5; 169 | axis->readParamInitialized=smtrue; 170 | } 171 | 172 | if(numFillPoints>=1)//send first fill data 173 | { 174 | smAppendSMCommandToQueue(axis->bushandle,SM_WRITE_VALUE_32B,fillPoints[0]); 175 | bytesUsed+=4; 176 | axis->numberOfPendingReadPackets+=1; 177 | } 178 | 179 | //send rest of data as increments 180 | if(numFillPoints>=2) 181 | { 182 | int i; 183 | //smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_ABSOLUTE_SETPOINT); 184 | //smAppendSMCommandToQueue(axis->bushandle,SMPCMD_SETPARAMADDR,SMP_INCREMENTAL_SETPOINT); 185 | //axis->numberOfPendingReadPackets++;//FIXME ei toimi ihan oikein tää koska skippaa äskeisen writevaluen 186 | 187 | for(i=1;ibushandle,SM_WRITE_VALUE_24B, fillPoints[i]-fillPoints[i-1] ); 190 | smAppendSMCommandToQueue(axis->bushandle,SM_WRITE_VALUE_32B,fillPoints[i]); 191 | bytesUsed+=4; 192 | axis->numberOfPendingReadPackets++; 193 | } 194 | } 195 | 196 | //send the commands that were added with smAppendSMCommandToQueue. this also reads all return packets that are available (executed already) 197 | smUploadCommandQueueToDeviceBuffer(axis->bushandle,axis->deviceAddress); 198 | 199 | //read all available return data from stream (commands that have been axecuted in drive so far) 200 | //return data works like FIFO for all sent commands (each sent stream command will produce return data packet that we fetch here) 201 | { 202 | smint32 bufferedReturnBytesReceived,readval; 203 | int n=0; 204 | 205 | //read return data buffer 206 | smBytesReceived(axis->bushandle,&bufferedReturnBytesReceived);//get amount of data available 207 | //qDebug()<1)//loop until we have read it all 209 | { 210 | smGetQueuedSMCommandReturnValue(axis->bushandle, &readval); 211 | smBytesReceived(axis->bushandle,&bufferedReturnBytesReceived); 212 | 213 | if(axis->numberOfDiscardableReturnDataPackets>0) 214 | { 215 | //discard this return data as it's intialization return packets 216 | axis->numberOfDiscardableReturnDataPackets--; 217 | } 218 | else//its read data that user expects 219 | { 220 | receivedPoints[n]=readval; 221 | n++; 222 | axis->numberOfPendingReadPackets--; 223 | } 224 | } 225 | *numReceivedPoints=n; 226 | } 227 | 228 | *bytesFilled=bytesUsed; 229 | return getCumulativeStatus(axis->bushandle); 230 | } 231 | 232 | /** this will stop executing buffered motion immediately and discard rest of already filled buffer on a given axis. May cause drive fault state such as tracking error if done at high speed because stop happens without deceleration.*/ 233 | SM_STATUS smBufferedAbort(BufferedMotionAxis *axis) 234 | { 235 | return smSetParameter( axis->bushandle, axis->deviceAddress, SMP_SYSTEM_CONTROL,SMP_SYSTEM_CONTROL_ABORTBUFFERED); 236 | } 237 | 238 | -------------------------------------------------------------------------------- /bufferedmotion.h: -------------------------------------------------------------------------------- 1 | #ifndef BUFFEREDMOTION_H 2 | #define BUFFEREDMOTION_H 3 | 4 | #ifdef __cplusplus 5 | extern "C"{ 6 | #endif 7 | 8 | #include "simplemotion.h" 9 | 10 | //typedef enum _smBufferedState {BufferedStop=0,BufferedRun=1} smBufferedState; 11 | 12 | typedef struct _BufferedMotionAxis { 13 | smbool initialized; 14 | smbool readParamInitialized; 15 | smint32 numberOfDiscardableReturnDataPackets; 16 | smint32 numberOfPendingReadPackets;//number of read data packets that should be arriving from device (to read rest of pending data, use smBufferedFillAndReceive(numFillPoints=0) until this variable this goes to zero) 17 | smbus bushandle; 18 | smaddr deviceAddress; 19 | smint32 samplerate; 20 | smint16 readParamAddr; 21 | smuint8 readParamLength; 22 | smint32 driveFlagsBeforeInit; 23 | smbool driveFlagsModifiedAtInit;//true if deInit should restore driveFlagsBeforeInit 24 | smint32 driveAccelerationBeforeInit; 25 | smuint16 driveClock;//clock counter is updated at smBufferedRunAndSyncClocks only for the one axis that is used with that func. clock is running up at 10kHz count rate, meaning that it rolls over every 6.5536 secs 26 | smint32 bufferLength;//buffer lenght in bytes of the device. note this may be different in different devices types. so call smBufferedGetFree on the device that has the smallest buffer. however as of 2.2016 all GD drives have 2048 bytes buffers. 27 | smint32 bufferFreeBytes;//number of bytes free in buffer, updated at smBufferedGetFree 28 | smint32 bufferFill;//percentage of buffer fill, updated at smBufferedGetFree. this should stay above 50% to ensure gapless motion. if gaps occur, check SMV2USB adpater COM port latency setting (set to 1ms) or try lower samplerate. 29 | smint32 smProtocolVersion;//version of SM protocol of the target device. some internal functionality of API may use this info. 30 | smint32 deviceCapabilityFlags1;//value of SMP_DEVICE_CAPABILITIES1 if target device has SM protocol version 28 or later (if SM version<28, then value is 0) 31 | smint32 deviceCapabilityFlags2;//value of SMP_DEVICE_CAPABILITIES2 if target device has SM protocol version 28 or later (if SM version<28, then value is 0) 32 | } BufferedMotionAxis; 33 | 34 | /** initialize buffered motion for one axis with address and samplerate (Hz) */ 35 | /* returnDataLenght must be one of following: 36 | #define SM_RETURN_STATUS 3 37 | //return value contains a read value from address defined by SMP_RETURN_PARAM_ADDR 38 | //consumes 2 byte from payload buffer. can contain value up to 14 bits. value greater than 14 bits is clipped (padded with 0s) 39 | #define SM_RETURN_VALUE_16B 2 40 | //return value contains a read value from address defined by SMP_RETURN_PARAM_ADDR 41 | //consumes 3 byte from payload buffer. can contain value up to 14 bits. value greater than 22 bits is clipped (padded with 0s) 42 | #define SM_RETURN_VALUE_24B 1 43 | //return value contains a read value from address defined by SMP_RETURN_PARAM_ADDR 44 | //consumes 4 byte from payload buffer. can contain value up to 30 bits. value greater than 14 bits is clipped (padded with 0s) 45 | #define SM_RETURN_VALUE_32B 0 46 | 47 | Note return data per one FillAndReceive must not exceed 120 bytes. So max allowed numFillPoints will depend on returnDataLength. 48 | numFillPoints must be equal or below 30 for 32B, 40 for 24B and 60 for 16B. 49 | */ 50 | LIB SM_STATUS smBufferedInit( BufferedMotionAxis *newAxis, smbus handle, smaddr deviceAddress, smint32 sampleRate, smint16 readParamAddr, smuint8 readDataLength ); 51 | 52 | /** uninitialize axis from buffered motion, recommended to call this before closing bus so drive's adjusted parameters are restored to originals*/ 53 | LIB SM_STATUS smBufferedDeinit( BufferedMotionAxis *axis ); 54 | 55 | /* this also starts buffered motion when it's not running*/ 56 | LIB SM_STATUS smBufferedRunAndSyncClocks( BufferedMotionAxis *axis ); 57 | LIB SM_STATUS smBufferedGetFree(BufferedMotionAxis *axis, smint32 *numBytesFree ); 58 | LIB smint32 smBufferedGetMaxFillSize(BufferedMotionAxis *axis, smint32 numBytesFree ); 59 | LIB smint32 smBufferedGetBytesConsumed(BufferedMotionAxis *axis, smint32 numFillPoints ); 60 | LIB SM_STATUS smBufferedFillAndReceive( BufferedMotionAxis *axis, smint32 numFillPoints, smint32 *fillPoints, smint32 *numReceivedPoints, smint32 *receivedPoints, smint32 *bytesFilled ); 61 | /** This will stop executing buffered motion immediately and discard rest of already filled buffer on a given axis. May cause drive fault state such as tracking error if done at high speed because stop happens without deceleration. 62 | Note: this will not stop motion, but just stop executing the sent buffered commands. The last executed motion point will be still followed by drive. So this is bad function 63 | for quick stopping stopping, for stop to the actual place consider using disable drive instead (prefferably phsyical input disable). 64 | */ 65 | LIB SM_STATUS smBufferedAbort(BufferedMotionAxis *axis); 66 | 67 | 68 | #ifdef __cplusplus 69 | } 70 | #endif 71 | #endif // BUFFEREDMOTION_H 72 | -------------------------------------------------------------------------------- /busdevice.c: -------------------------------------------------------------------------------- 1 | #include "busdevice.h" 2 | #include "user_options.h" 3 | 4 | #include "drivers/serial/pcserialport.h" 5 | #include "drivers/tcpip/tcpclient.h" 6 | #include "drivers/ftdi_d2xx/sm_d2xx.h" 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | 13 | //how much bytes available in transmit buffer 14 | #define TANSMIT_BUFFER_LENGTH 128 15 | 16 | unsigned long SMBusBaudrate=SM_BAUDRATE; //the next opened port (with smOpenBus) will be opened with the PBS defined here (default 460800 BPS) 17 | 18 | typedef struct _SMBusDevice 19 | { 20 | //common 21 | smbool opened; 22 | 23 | SM_STATUS cumulativeSmStatus; 24 | 25 | //pointer used by bus device drivers 26 | smBusdevicePointer busDevicePointer; 27 | 28 | smuint8 txBuffer[TANSMIT_BUFFER_LENGTH]; 29 | smint32 txBufferUsed;//how many bytes in buffer currently 30 | 31 | BusdeviceOpen busOpenCallback; 32 | BusdeviceReadBuffer busReadCallback; 33 | BusdeviceWriteBuffer busWriteCallback; 34 | BusdeviceMiscOperation busMiscOperationCallback; 35 | BusdeviceClose busCloseCallback; 36 | } SMBusDevice; 37 | 38 | //init on first open 39 | smbool bdInitialized=smfalse; 40 | SMBusDevice BusDevice[SM_MAX_BUSES]; 41 | 42 | //init device struct table 43 | void smBDinit() 44 | { 45 | int i; 46 | for(i=0;i=0) return h;//was success 65 | h=smBDOpenWithCallbacks( devicename, tcpipPortOpen, tcpipPortClose, tcpipPortRead, tcpipPortWrite, tcpipMiscOperation ); 66 | if(h>=0) return h;//was success 67 | #ifdef FTDI_D2XX_SUPPORT 68 | h=smBDOpenWithCallbacks( devicename, d2xxPortOpen, d2xxPortClose, d2xxPortRead, d2xxPortWrite, d2xxPortMiscOperation ); 69 | if(h>=0) return h;//was success 70 | #endif 71 | #else 72 | smDebug( -1, SMDebugHigh, "smBDOpen ENABLE_BUILT_IN_DRIVERS is not defined during SM library compile time. smOpenBus not supported in this case, see README.md."); 73 | #endif 74 | 75 | //none succeeded 76 | //smDebug( -1, Low, "smBDOpen device name argument syntax didn't match any supported driver port name"); 77 | return -1; 78 | } 79 | 80 | 81 | 82 | smbusdevicehandle smBDOpenWithCallbacks(const char *devicename, BusdeviceOpen busOpenCallback, BusdeviceClose busCloseCallback , BusdeviceReadBuffer busReadCallback, BusdeviceWriteBuffer busWriteCallback, BusdeviceMiscOperation busMiscOperationCallback ) 83 | { 84 | int handle; 85 | smbool success; 86 | 87 | //true on first call 88 | if(bdInitialized==smfalse) 89 | smBDinit(); 90 | 91 | //find free handle 92 | for(handle=0;handle=SM_MAX_BUSES) return -1; 99 | 100 | //setup callbacks 101 | BusDevice[handle].busOpenCallback=busOpenCallback; 102 | BusDevice[handle].busWriteCallback=busWriteCallback; 103 | BusDevice[handle].busReadCallback=busReadCallback; 104 | BusDevice[handle].busMiscOperationCallback=busMiscOperationCallback; 105 | BusDevice[handle].busCloseCallback=busCloseCallback; 106 | 107 | //try opening 108 | BusDevice[handle].busDevicePointer=BusDevice[handle].busOpenCallback( devicename, SMBusBaudrate, &success ); 109 | if( success==smfalse ) 110 | { 111 | return -1; //failed to open 112 | } 113 | 114 | BusDevice[handle].opened=smtrue; 115 | BusDevice[handle].txBufferUsed=0; 116 | BusDevice[handle].cumulativeSmStatus=0; 117 | 118 | //purge 119 | if(smBDMiscOperation(handle,MiscOperationPurgeRX)==smfalse) 120 | { 121 | smBDClose(handle); 122 | return -1; //failed to purge 123 | } 124 | 125 | //success 126 | return handle; 127 | } 128 | 129 | smbool smIsBDHandleOpen( const smbusdevicehandle handle ) 130 | { 131 | if(handle<0) return smfalse; 132 | if(handle>=SM_MAX_BUSES) return smfalse; 133 | return BusDevice[handle].opened; 134 | } 135 | 136 | //return true if ok 137 | smbool smBDClose( const smbusdevicehandle handle ) 138 | { 139 | //check if handle valid & open 140 | if( smIsBDHandleOpen(handle)==smfalse ) return smfalse; 141 | 142 | BusDevice[handle].busCloseCallback(BusDevice[handle].busDevicePointer ); 143 | BusDevice[handle].opened=smfalse; 144 | return smtrue; 145 | } 146 | 147 | 148 | 149 | 150 | //write one byte to buffer and send later with smBDTransmit() 151 | //returns true on success 152 | smbool smBDWrite(const smbusdevicehandle handle, const smuint8 byte ) 153 | { 154 | //check if handle valid & open 155 | if( smIsBDHandleOpen(handle)==smfalse ) return smfalse; 156 | 157 | if(BusDevice[handle].txBufferUsed 2 | 3 | 4 | 18 | 20 | 43 | 50 | 51 | 53 | 54 | 56 | image/svg+xml 57 | 59 | 60 | 61 | 62 | 63 | 68 | 75 | 82 | 89 | 96 | 103 | Subpacket 1 115 | 1 bytePackettype 143 | 0 or 1 bytesNumber of payloadbytes unlessfixed lenght defined by packet type 176 | 1 byteTarget deviceaddress 1-255 or 0 if broadcast to all devices 209 | 0-120 bytesPayload datamade ofsubpackets 237 | 2 byteCRC checksum 260 | 267 | 274 | 2 bitssubpackettype andlenght 302 | 6, 14, 22 or 30 bitsValue data(write / read valueof parameter) 330 | Subpacket N 342 | 349 | 356 | 2 bitssubpackettype andlenght 384 | 6, 14, 22 or 30 bitsValue data(write / read valueof parameter) 412 | . . . 423 | 429 | 435 | SimpleMotion V2 outbound or inbound packet 447 | 458 | 3 - 125 bytes total 470 | 477 | Packet typesOutbound (host to devices):- execute payload subpackets instantly- place payload subpackets in buffer executed by timed fashion- get device clock value (used to sync buffered execution)Inbound (device to host)- Return data packets (one type for each outbound type), these contain payload data (read values) or clock value 540 | 547 | Subpacket typesOutbound- set parameter address where next values are written to, 2 bytes long- write parameter value, 3 or 4 bytes longInbound- SM bus status value, 1 byte long- read parameter value, 2, 3 or 4 bytes longEach outbound subpacket will give one inbound subpacket in return when devicesends the response to a SM packet. Content of inbound subpackets are defined bywritable parameters SMP_RETURN_PARAM_ADDR and SMP_RETURN_PARAM_LEN. 630 | SimpleMotion V2 Protocol 642 | 643 | 644 | -------------------------------------------------------------------------------- /doc/simplemotion protocol subpacket flow diagram.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Draw.io Diagram 5 | 6 | 7 | 8 | 9 |
10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /doc/simplemotion protocol subpacket flow diagram.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GraniteDevices/SimpleMotionV2/b890f7f99724c66a55915125bb70ddb59ea7e346/doc/simplemotion protocol subpacket flow diagram.pdf -------------------------------------------------------------------------------- /drivers/ftdi_d2xx/sm_d2xx.c: -------------------------------------------------------------------------------- 1 | /* 2 | * ftdi_d2xx.c 3 | * 4 | * Header for FTDI D2XX serial port access library 5 | * 6 | * Created on: 19.8.2017 7 | * Author: Tero Kontkanen 8 | */ 9 | 10 | #include "drivers/ftdi_d2xx/sm_d2xx.h" 11 | #include "simplemotion_private.h" //needed for timeout variable 12 | #include "drivers/ftdi_d2xx/third_party/ftd2xx.h" 13 | #include "user_options.h" 14 | #include 15 | #include 16 | 17 | 18 | static int stringToNumber( const char *str, smbool *ok ) 19 | { 20 | int len=strlen(str); 21 | int i, number=0, decade=1; 22 | 23 | if(len<1) 24 | { 25 | *ok=smfalse; 26 | return 0; 27 | } 28 | 29 | for(i=len-1;i>=0;i--) 30 | { 31 | if(str[i]<'0' || str[i]>'9')//non-numeric char->fail 32 | { 33 | *ok=smfalse; 34 | return 0; 35 | } 36 | number+=decade*(str[i]-'0'); 37 | decade*=10; 38 | } 39 | *ok=smtrue; 40 | return number; 41 | } 42 | 43 | smBusdevicePointer d2xxPortOpen(const char *port_device_name, smint32 baudrate_bps, smbool *success) 44 | { 45 | *success=smfalse; 46 | 47 | //parse name string 48 | int ftdiIndex=0; 49 | smbool ftdiIndexParseOk=smfalse; 50 | 51 | if (strncmp(port_device_name,"FTDI",4) == 0)//must start with FTDI. Full name is FTDIn where n=index starting from 0. 52 | { 53 | ftdiIndex=stringToNumber(port_device_name+4,&ftdiIndexParseOk); 54 | } 55 | 56 | //open port 57 | FT_HANDLE h; 58 | FT_STATUS s; 59 | 60 | if(ftdiIndexParseOk==smtrue)//if name suggests that we should try opening by index 61 | s=FT_Open(ftdiIndex,&h); 62 | else//try to open by device name instead 63 | s=FT_OpenEx((char*)port_device_name,FT_OPEN_BY_DESCRIPTION,&h); 64 | 65 | if(s==FT_OK) 66 | { 67 | if(FT_ResetDevice(h)!=FT_OK) 68 | { 69 | smDebug( -1, SMDebugLow, "FTDI port error: failed to reset USB chip\n"); 70 | goto error; 71 | } 72 | 73 | //init port settings 74 | s=FT_SetBaudRate(h,baudrate_bps); 75 | if(s!=FT_OK) 76 | { 77 | smDebug( -1, SMDebugLow, "FTDI port error: failed to set baud rate\n"); 78 | goto error; 79 | } 80 | 81 | if(FT_SetLatencyTimer(h,1)!=FT_OK)//API doc says 2ms is minimum but 1 seem to work too 82 | { 83 | smDebug( -1, SMDebugLow, "FTDI port error: failed to set latency\n"); 84 | goto error; 85 | } 86 | 87 | if(FT_SetFlowControl(h,FT_FLOW_NONE,0,0)!=FT_OK) 88 | { 89 | smDebug( -1, SMDebugLow, "FTDI port error: failed to set flow control\n"); 90 | goto error; 91 | } 92 | 93 | if(FT_SetDataCharacteristics(h,FT_BITS_8,FT_STOP_BITS_1,FT_PARITY_NONE)!=FT_OK) 94 | { 95 | smDebug( -1, SMDebugLow, "FTDI port error: failed to set data characteristics\n"); 96 | goto error; 97 | } 98 | 99 | /* Workaround FTDI D2XX bug where first FT_Reads will fail with timeout even when there IS data available. 100 | * In the workaround we set timeout long enough and try read one byte and let it timeout. After this consequent FT_Reads seem to work properly. 101 | * 250 ms seemed to be barely enough, so use 350ms to for some safety margin. 102 | */ 103 | if(FT_SetTimeouts(h,350,350)!=FT_OK) 104 | { 105 | smDebug( -1, SMDebugLow, "FTDI port error: failed to set workaround timeout\n"); 106 | goto error; 107 | } 108 | else 109 | { 110 | DWORD BytesReceived; 111 | char buf[1]; 112 | if(FT_Read(h,buf,1,&BytesReceived)!=FT_OK) //we expect this to return 0 bytes after 350ms timeout 113 | { 114 | smDebug( -1, SMDebugLow, "FTDI port error: FT_Read workaround failed\n"); 115 | goto error; 116 | } 117 | } 118 | 119 | if(FT_SetTimeouts(h,readTimeoutMs,readTimeoutMs)!=FT_OK) 120 | { 121 | smDebug( -1, SMDebugLow, "FTDI port error: failed to set timeout\n"); 122 | goto error; 123 | } 124 | 125 | if(FT_Purge(h,FT_PURGE_RX|FT_PURGE_TX)!=FT_OK) 126 | { 127 | smDebug( -1, SMDebugLow, "FTDI port error: failed to set purge\n"); 128 | goto error; 129 | } 130 | 131 | smDebug( -1, SMDebugMid, "FTDI port opened\n"); 132 | *success=smtrue; 133 | return (smBusdevicePointer)h; 134 | 135 | smDebug( -1, SMDebugLow, "FTDI port error: all handles taken, too many ports open\n"); 136 | goto error; 137 | } 138 | else 139 | { 140 | if(ftdiIndexParseOk==smtrue) 141 | { 142 | smDebug( -1, SMDebugLow, "FTDI port error: FT_Open with name %s (index %d) failed\n", port_device_name, ftdiIndex); 143 | } 144 | else 145 | { 146 | smDebug( -1, SMDebugLow, "FTDI port error: FT_OpenEx with device description %s failed.\n", port_device_name); 147 | smDebug( -1, SMDebugLow, "FTDI port error additional info: FTDI D2XX port open attempt was made by device description because smOpenBus() argument format was not FTDIn where index number (0 or greater), or any other supported port name format.\n"); 148 | } 149 | } 150 | 151 | error: 152 | FT_Close(h); 153 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 154 | } 155 | 156 | 157 | smint32 d2xxPortRead(smBusdevicePointer busdevicepointer, unsigned char *buf, smint32 size) 158 | { 159 | FT_HANDLE hanlde=(FT_HANDLE)busdevicepointer; 160 | FT_STATUS s; 161 | DWORD BytesReceived; 162 | 163 | s=FT_Read(hanlde,buf,size,&BytesReceived); 164 | if(s!=FT_OK) 165 | { 166 | //failed 167 | smDebug( -1, SMDebugLow, "FTDI port error: failed to receive data from port\n"); 168 | } 169 | 170 | return BytesReceived; 171 | } 172 | 173 | 174 | 175 | smint32 d2xxPortWrite(smBusdevicePointer busdevicepointer, unsigned char *buf, smint32 size) 176 | { 177 | FT_HANDLE handle=(FT_HANDLE)busdevicepointer; 178 | DWORD BytesWritten; 179 | FT_STATUS s=FT_Write(handle, buf, size, &BytesWritten); 180 | 181 | if(s!=FT_OK) 182 | { 183 | //failed 184 | smDebug( -1, SMDebugLow, "FTDI port error: failed to write data to port\n"); 185 | } 186 | 187 | return BytesWritten; 188 | } 189 | 190 | smbool d2xxPortMiscOperation(smBusdevicePointer busdevicePointer, BusDeviceMiscOperationType operation) 191 | { 192 | FT_HANDLE handle=(FT_HANDLE)busdevicePointer; 193 | 194 | switch(operation) 195 | { 196 | case MiscOperationPurgeRX: 197 | if(FT_Purge(handle,FT_PURGE_RX)!=FT_OK) 198 | { 199 | smDebug( -1, SMDebugLow, "FTDI port error: failed to purge\n"); 200 | return smfalse; 201 | } 202 | return smtrue; 203 | break; 204 | case MiscOperationFlushTX: 205 | { 206 | int timespent_ms=0; 207 | while(1) 208 | { 209 | DWORD InRxQueue, InTxQueue, EventStatus; 210 | FT_STATUS stat=FT_GetStatus(handle, &InRxQueue, &InTxQueue, &EventStatus); 211 | 212 | if(stat!=FT_OK) 213 | { 214 | smDebug( -1, SMDebugLow, "FTDI port error: FT_GetStatus failed\n"); 215 | return smfalse; 216 | } 217 | 218 | if(InTxQueue==0)//no data to be sent in buffer 219 | return smtrue; 220 | 221 | //wait before next loop round 222 | smSleepMs(1); 223 | timespent_ms++; 224 | if(timespent_ms>=readTimeoutMs) 225 | return smfalse;//timeouted 226 | }; 227 | } 228 | break; 229 | default: 230 | smDebug( -1, SMDebugLow, "FTDI port error: given MiscOperataion not implemented\n"); 231 | return smfalse; 232 | break; 233 | } 234 | } 235 | 236 | 237 | void d2xxPortClose(smBusdevicePointer busdevicepointer) 238 | { 239 | FT_HANDLE handle=(FT_HANDLE)busdevicepointer; 240 | 241 | if(FT_Close(handle)!=FT_OK) 242 | { 243 | //failed 244 | smDebug( -1, SMDebugLow, "FTDI port error: failed to close port\n"); 245 | } 246 | } 247 | 248 | 249 | 250 | //BUS DEVICE INFO FETCH FUNCTIONS: 251 | 252 | //Return number of bus devices found. details of each device may be consequently fetched by smGetBusDeviceDetails() 253 | smint d2xxGetNumberOfDetectedBuses() 254 | { 255 | FT_STATUS ftStatus; 256 | DWORD numDevs; 257 | // Get the number of devices currently connected 258 | ftStatus = FT_ListDevices(&numDevs,NULL,FT_LIST_NUMBER_ONLY); 259 | if (ftStatus == FT_OK) 260 | { 261 | return numDevs; 262 | } 263 | else 264 | { 265 | return 0; 266 | } 267 | 268 | return 0; 269 | } 270 | 271 | smbool d2xxGetBusDeviceDetails( smint index, SM_BUS_DEVICE_INFO *info ) 272 | { 273 | DWORD devIndex = index; 274 | char description[64]; // more than enough room 275 | FT_STATUS ftStatus = FT_ListDevices((PVOID)devIndex, description, FT_LIST_BY_INDEX|FT_OPEN_BY_DESCRIPTION); 276 | if (ftStatus == FT_OK) 277 | { 278 | smbool compatible=smfalse; 279 | if(strncmp(description,"TTL232R",7)==0 280 | || strncmp(description,"SMV2USB",7)==0 281 | || strncmp(description,"USB-SMV2",8)==0 282 | || strncmp(description,"FT230X",6)==0 283 | || strncmp(description,"IONICUBE",8)==0 284 | || strncmp(description,"ATOMI",5)==0) 285 | compatible=smtrue; 286 | 287 | info->is_simplemotion_device=compatible; 288 | 289 | if(compatible==smtrue) 290 | { 291 | sprintf(info->description,"SimpleMotion USB (%s)",description); 292 | sprintf(info->device_name,"FTDI%d",index); 293 | } 294 | else//some unknown device with FTDI chip 295 | { 296 | sprintf(info->description,"Unknown FTDI device (%s)",description); 297 | sprintf(info->device_name,"FTDI%d",index); 298 | } 299 | return smtrue; 300 | } 301 | else 302 | { 303 | return smfalse; 304 | } 305 | 306 | return smfalse; 307 | } 308 | 309 | -------------------------------------------------------------------------------- /drivers/ftdi_d2xx/sm_d2xx.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ftdi_d2xx.h 3 | * 4 | * Header for FTDI D2XX serial port access library 5 | * 6 | * Created on: 19.8.2017 7 | * Author: Tero Kontkanen 8 | */ 9 | 10 | 11 | #ifndef SM_D2XX_H 12 | #define SM_D2XX_H 13 | 14 | #include "simplemotion_private.h" 15 | 16 | #ifdef __cplusplus 17 | extern "C" { 18 | #endif 19 | 20 | #include "simplemotion.h" 21 | 22 | //max number of simultaneously connected FTDI devices 23 | #define MAX_OPEN_PORTS 32 24 | 25 | smBusdevicePointer d2xxPortOpen(const char *port_device_name, smint32 baudrate_bps, smbool *success); 26 | smint32 d2xxPortRead(smBusdevicePointer busdevicepointer, unsigned char *buf, smint32 size); 27 | smint32 d2xxPortWrite(smBusdevicePointer busdevicepointer, unsigned char *buf, smint32 size); 28 | smbool d2xxPortMiscOperation(smBusdevicePointer busdevicePointer, BusDeviceMiscOperationType operation); 29 | void d2xxPortClose(smBusdevicePointer busdevicepointer); 30 | 31 | //Return number of bus devices found. details of each device may be consequently fetched by smGetBusDeviceDetails() 32 | smint d2xxGetNumberOfDetectedBuses(); 33 | smbool d2xxGetBusDeviceDetails( smint index, SM_BUS_DEVICE_INFO *info ); 34 | 35 | #ifdef __cplusplus 36 | } 37 | #endif 38 | 39 | #endif 40 | 41 | 42 | -------------------------------------------------------------------------------- /drivers/ftdi_d2xx/third_party/Readme and license.txt: -------------------------------------------------------------------------------- 1 | Files in this folder are downloaded from FTDI: 2 | http://www.ftdichip.com/Drivers/D2XX.htm 3 | 4 | 5 | Driver license 6 | -------------- 7 | 8 | IMPORTANT NOTICE: PLEASE READ CAREFULLY BEFORE INSTALLING THE RELEVANT SOFTWARE: This licence agreement ("Licence") is a legal agreement between you ("Licencee" or "you") and Future Technology Devices International Limited of 2 Seaward Place, Centurion Business Park, Glasgow, Scotland, G41 1HH (UK Company Number SC136640) ("Licensor" or "we) for use of driver software provided by the Licensor ("Software"). 9 | 10 | BY INSTALLING OR USING THIS SOFTWARE YOU AGREE TO THE TERMS OF THIS LICENCE WHICH WILL BIND YOU. IF YOU DO NOT AGREE TO THE TERMS OF THIS LICENCE, WE ARE UNWILLING TO LICENSE THE SOFTWARE TO YOU AND YOU MUST DISCONTINUE INSTALLATION OF THE SOFTWARE NOW. 11 | 12 | 1. Grant and scope of licence 13 | 14 | 1.1 In consideration of you agreeing to abide by the terms of this Licence, the Licensor hereby grants to you a non-exclusive, non-transferable, royalty free licence to use the Software on the terms of this Licence. 15 | 16 | 1.2 In this Licence a "Genuine FTDI Component" means an item of hardware that was manufactured for, and sold by, the Licensor or a member of the Licensor's group of companies. It does not include any counterfeit or fake products. 17 | 18 | 1.3 If you are a manufacturer of a device that includes a Genuine FTDI Component (each a "Device") then you may install the Software onto that device. If you are a seller or distributor of a Device then You may distribute the Software with the Device. If you are a user of a Device then you may install the Software on the Device, or onto a computer system in order to use the Device. 19 | 20 | 1.4 In each of those cases you may: 21 | 22 | 1.4.1 install and use the Software for your purposes only; and 23 | 24 | 1.4.2 only use the Software in conjunction with products based on and/or incorporating a Genuine FTDI Component. 25 | 26 | 1.5 The Software will not function properly on or with a component that is not a Genuine FTDI Component. Use of the Software as a driver for, or installation of the Software onto, a component that is not a Genuine FTDI Component, including without limitation counterfeit components, MAY IRRETRIEVABLY DAMAGE THAT COMPONENT. It is the Licensee's responsibility to make sure that all chips it installs the Software on, or uses the Software as a driver for, are Genuine FTDI Components. If in doubt then contact the Licensor. 27 | 28 | 2. If a custom vendor ID and/or product ID or description string are used, it is the responsibility of the product manufacturer to maintain any changes and subsequent WHQL re-certification as a result of making these changes. 29 | 30 | 3. Licensee's undertakings 31 | 32 | 3.1 Except as expressly set out in this Licence or as permitted by any local law, you undertake: 33 | 34 | 3.1.1 not to copy the Software, except where such copying is incidental to normal use of the Software or where it is necessary for the purpose of back-up or operational security; 35 | 36 | 3.1.2 not to rent, lease, sub-license, loan, translate, merge, adapt, vary or modify the Software or any part of it; 37 | 38 | 3.1.3 not to make alterations to, or modifications of, the whole or any part of the Software nor permit the Software or any part of it to be combined with, or become incorporated in, any other programs; 39 | 40 | 3.1.4 not to disassemble, de-compile, reverse engineer or create derivative works based on the whole or any part of the Software; 41 | 42 | 3.1.5 to keep all copies of the Software secure; 43 | 44 | 3.1.6 to include the copyright notice of the Licensor on all entire and partial copies of the Software in any form; and 45 | 46 | 3.1.7 not to provide, or otherwise make available, the Software in any form, in whole or in part (including, but not limited to, program listings, object and source program listings, object code and source code) to any person. 47 | 48 | 4. Intellectual property rights 49 | 50 | 4.1 You acknowledge that all intellectual property rights in the Software throughout the world belong to the Licensor, that rights in the Software are licensed (not sold) to you, and that you have no rights in, or to, the Software other than the right to use them in accordance with the terms of this Licence. 51 | 52 | 5. Warranty 53 | 54 | 5.1 To the maximum extent permitted by applicable law, the software is provided "as is". 55 | 56 | 5.2 All implied warranties, implied conditions and/or implied licences are excluded from this Licence, including but not limited to implied warranties of quality and/or fitness for purpose (in all cases) to the fullest extent permitted by law. 57 | 58 | 59 | 60 | 5.3 You acknowledge that the Software has not been developed to meet your individual requirements and that the Software may not be uninterrupted or free of bugs or errors. 61 | 62 | 6. Licensor's liability 63 | 64 | 6.1 To the maximum extent permitted by applicable law, in no event shall the Licensor be liable for any: 65 | 66 | 6.1.1 special loss or damage; 67 | 68 | 6.1.2 incidental loss or damage; 69 | 70 | 6.1.3 indirect or consequential loss or damage: 71 | 72 | 6.1.4 loss of income; 73 | 74 | 6.1.5 loss of business; 75 | 76 | 6.1.6 loss of profits; 77 | 78 | 6.1.7 loss of revenue; 79 | 80 | 6.1.8 loss of contracts; 81 | 82 | 6.1.9 business interruption; 83 | 84 | 6.1.10 loss of the use of money or anticipated savings; 85 | 86 | 6.1.11 loss of information; 87 | 88 | 6.1.12 loss of opportunity; 89 | 90 | 6.1.13 loss of goodwill or reputation; and/or 91 | 92 | 6.1.14 loss of, damage to or corruption of data; 93 | 94 | (in each case) of any kind howsoever arising and whether caused by delict (including negligence), breach of contract or otherwise. 95 | 96 | 6.2 FTDI's total liability to you in relation to the Software shall not exceed 500 US Dollars. 97 | 98 | 6.3 Nothing in this Licence limits or excludes liability for death or personal injury or for fraud. 99 | 100 | 7. Termination 101 | 102 | 7.1 The Licensor may terminate this Licence immediately if: 103 | 104 | 7.1.1 you fail to comply with any of the terms and conditions of the Licence; or 105 | 106 | 7.1.2 you commence or participate in any legal proceedings against the Licensor. 107 | 108 | 7.2 Upon termination: 109 | 110 | 7.2.1 all rights granted to you under this Licence shall cease; 111 | 112 | 7.2.2 you must cease all activities authorised by this Licence; and 113 | 114 | 7.2.3 you must immediately delete or remove the Software from all computer equipment in your possession and immediately destroy all copies of the Software then in your possession, custody or control. 115 | 116 | 8. Transfer of rights and obligations 117 | 118 | 8.1 You may not transfer, assign, charge or otherwise dispose of this Licence, or any of your rights or obligations arising under it. 119 | 120 | 8.2 The Licensor may transfer, assign, charge, sub-contract or otherwise dispose of this Licence, or any of his rights or obligations arising under it, at any time during the term of the Licence. 121 | 122 | 9. Waiver 123 | 124 | 9.1 If the Licensor fails, at any time during the term of this Licence, to insist on strict performance of any of your obligations under this Licence, or if the Licensor fails to exercise any of the rights or remedies to which he is entitled under this Licence, this shall not constitute a waiver of such rights or remedies and shall not relieve you from compliance with such obligations. 125 | 126 | 9.2 A waiver by the Licensor of any default shall not constitute a waiver of any subsequent default. 127 | 128 | 9.3 No waiver by the Licensor of any of these terms and conditions shall be effective unless it is expressly stated to be a waiver and is communicated to you in writing. 129 | 130 | 10. Severability 131 | 132 | If any of the terms of this Licence are determined by any competent authority to be invalid, unlawful or unenforceable to any extent, such term, condition or provision will to that extent be severed from the remaining terms, conditions and provisions which will continue to be valid to the fullest extent permitted by law. 133 | 134 | 11. Entire agreement 135 | 136 | 11.1 This Licence constitutes the whole agreement between us and supersedes any previous arrangement, understanding or agreement between us, relating to the licensing of the Software. 137 | 138 | 11.2 Each party acknowledges that in entering into this Licence it does not rely on any statement, representation, warranty or understanding other than those expressly set out in this Licence. Each party agrees that it will have no remedy in respect of any statement, representation, warranty or understanding that is not expressly set out in this Licence. Each party agrees that its only remedy in respect of those representations, statements, assurances and warranties that are set out in this Licence will be for breach of contract in accordance with the terms of this Licence. 139 | 140 | 11.3 The parties agree that nothing in this Licence will limit or exclude any liability they may have for fraud. 141 | 142 | 12. Miscellaneous 143 | 144 | 12.1 This Licence does not create a partnership or joint venture between the parties to it, nor authorise a party to act as agent for the other. 145 | 146 | 12.2 This Licence does not create any legal rights enforceable by any third party. 147 | 148 | 12.3 This Licence may only be varied by express written legal agreement between the parties. 149 | 150 | 13. Law and jurisdiction 151 | 152 | This Licence, its subject matter or its formation (including non-contractual disputes or claims) shall be governed by and construed in accordance with Scots law and submitted to the non-exclusive jurisdiction of the Scottish courts. 153 | 154 | 155 | 156 | OS X library 157 | ------------ 158 | 159 | OS X D2XX Library Version 1.4.4 160 | © Future Technology Devices International Ltd. 2016 161 | 162 | DISCLAIMER 163 | 164 | This software is supplied on an as-is basis and no warranty as to their suitability for any particular purpose is either made or implied. Future Technology Devices International Ltd. will not accept any claim for damages howsoever arising as a result of use or failure of this software. Your statutory rights are not affected. This software or any variant of it is not intended for use in any medical appliance, device or system in which the failure of the product might reasonably be expected to result in personal injury. This document provides preliminary information that may be subject to change without notice. 165 | 166 | Libusb 167 | 168 | This package uses an unmodified version of libusb (libusb.info) which is distributed under the terms of the GNU Lesser General Public License (see libusb/COPYING or www.gnu.org/licenses). Source code for libusb is included in this distribution. 169 | -------------------------------------------------------------------------------- /drivers/ftdi_d2xx/third_party/WinTypes.h: -------------------------------------------------------------------------------- 1 | #ifndef __WINDOWS_TYPES__ 2 | #define __WINDOWS_TYPES__ 3 | 4 | #define WINAPI 5 | 6 | typedef unsigned int DWORD; 7 | typedef unsigned int ULONG; 8 | typedef unsigned short USHORT; 9 | typedef unsigned short SHORT; 10 | typedef unsigned char UCHAR; 11 | typedef unsigned short WORD; 12 | typedef unsigned short WCHAR; 13 | typedef unsigned char BYTE; 14 | typedef BYTE *LPBYTE; 15 | typedef unsigned int BOOL; 16 | typedef unsigned char BOOLEAN; 17 | typedef unsigned char CHAR; 18 | typedef BOOL *LPBOOL; 19 | typedef UCHAR *PUCHAR; 20 | typedef const char *LPCSTR; 21 | typedef char *PCHAR; 22 | typedef void *PVOID; 23 | typedef void *HANDLE; 24 | typedef unsigned int LONG; 25 | typedef int INT; 26 | typedef unsigned int UINT; 27 | typedef char *LPSTR; 28 | typedef char *LPTSTR; 29 | typedef const char *LPCTSTR; 30 | typedef DWORD *LPDWORD; 31 | typedef WORD *LPWORD; 32 | typedef ULONG *PULONG; 33 | typedef LONG *LPLONG; 34 | typedef PVOID LPVOID; 35 | typedef void VOID; 36 | typedef USHORT *PUSHORT; 37 | typedef unsigned long long int ULONGLONG; 38 | 39 | typedef struct _OVERLAPPED { 40 | DWORD Internal; 41 | DWORD InternalHigh; 42 | union { 43 | struct{ 44 | DWORD Offset; 45 | DWORD OffsetHigh; 46 | }; 47 | PVOID Pointer; 48 | }; 49 | HANDLE hEvent; 50 | } OVERLAPPED, *LPOVERLAPPED; 51 | 52 | typedef struct _SECURITY_ATTRIBUTES { 53 | DWORD nLength; 54 | LPVOID lpSecurityDescriptor; 55 | BOOL bInheritHandle; 56 | } SECURITY_ATTRIBUTES , *LPSECURITY_ATTRIBUTES; 57 | 58 | #include 59 | // Substitute for HANDLE returned by Windows CreateEvent API. 60 | // FT_SetEventNotification expects parameter 3 to be the address 61 | // of one of these structures. 62 | typedef struct _EVENT_HANDLE 63 | { 64 | pthread_cond_t eCondVar; 65 | pthread_mutex_t eMutex; 66 | int iVar; 67 | } EVENT_HANDLE; 68 | 69 | typedef struct timeval SYSTEMTIME; 70 | typedef struct timeval FILETIME; 71 | 72 | // WaitForSingleObject return values. 73 | #define WAIT_ABANDONED 0x00000080L 74 | #define WAIT_OBJECT_0 0x00000000L 75 | #define WAIT_TIMEOUT 0x00000102L 76 | #define WAIT_FAILED 0xFFFFFFFF 77 | // Special value for WaitForSingleObject dwMilliseconds parameter 78 | #define INFINITE 0xFFFFFFFF // Infinite timeout 79 | 80 | #ifndef TRUE 81 | #define TRUE 1 82 | #endif 83 | #ifndef FALSE 84 | #define FALSE 0 85 | #endif 86 | #ifndef CONST 87 | #define CONST const 88 | #endif 89 | // 90 | // Modem Status Flags 91 | // 92 | #define MS_CTS_ON ((DWORD)0x0010) 93 | #define MS_DSR_ON ((DWORD)0x0020) 94 | #define MS_RING_ON ((DWORD)0x0040) 95 | #define MS_RLSD_ON ((DWORD)0x0080) 96 | 97 | // 98 | // Error Flags 99 | // 100 | #define CE_RXOVER 0x0001 // Receive Queue overflow 101 | #define CE_OVERRUN 0x0002 // Receive Overrun Error 102 | #define CE_RXPARITY 0x0004 // Receive Parity Error 103 | #define CE_FRAME 0x0008 // Receive Framing error 104 | #define CE_BREAK 0x0010 // Break Detected 105 | #define CE_TXFULL 0x0100 // TX Queue is full 106 | #define CE_PTO 0x0200 // LPTx Timeout 107 | #define CE_IOE 0x0400 // LPTx I/O Error 108 | #define CE_DNS 0x0800 // LPTx Device not selected 109 | #define CE_OOP 0x1000 // LPTx Out-Of-Paper 110 | #define CE_MODE 0x8000 // Requested mode unsupported 111 | 112 | // 113 | // Events 114 | // 115 | #define EV_RXCHAR 0x0001 // Any Character received 116 | #define EV_RXFLAG 0x0002 // Received certain character 117 | #define EV_TXEMPTY 0x0004 // Transmit Queue Empty 118 | #define EV_CTS 0x0008 // CTS changed state 119 | #define EV_DSR 0x0010 // DSR changed state 120 | #define EV_RLSD 0x0020 // RLSD changed state 121 | #define EV_BREAK 0x0040 // BREAK received 122 | #define EV_ERR 0x0080 // Line status error occurred 123 | #define EV_RING 0x0100 // Ring signal detected 124 | #define EV_PERR 0x0200 // Printer error occured 125 | #define EV_RX80FULL 0x0400 // Receive buffer is 80 percent full 126 | #define EV_EVENT1 0x0800 // Provider specific event 1 127 | #define EV_EVENT2 0x1000 // Provider specific event 2 128 | 129 | // 130 | // Escape Functions 131 | // 132 | #define SETXOFF 1 // Simulate XOFF received 133 | #define SETXON 2 // Simulate XON received 134 | #define SETRTS 3 // Set RTS high 135 | #define CLRRTS 4 // Set RTS low 136 | #define SETDTR 5 // Set DTR high 137 | #define CLRDTR 6 // Set DTR low 138 | #define RESETDEV 7 // Reset device if possible 139 | #define SETBREAK 8 // Set the device break line. 140 | #define CLRBREAK 9 // Clear the device break line. 141 | 142 | // 143 | // PURGE function flags. 144 | // 145 | #define PURGE_TXABORT 0x0001 // Kill the pending/current writes to the comm port. 146 | #define PURGE_RXABORT 0x0002 // Kill the pending/current reads to the comm port. 147 | #define PURGE_TXCLEAR 0x0004 // Kill the transmit queue if there. 148 | #define PURGE_RXCLEAR 0x0008 // Kill the typeahead buffer if there. 149 | 150 | #ifndef INVALID_HANDLE_VALUE 151 | #define INVALID_HANDLE_VALUE 0xFFFFFFFF 152 | #endif 153 | 154 | #endif /* __WINDOWS_TYPES__ */ 155 | -------------------------------------------------------------------------------- /drivers/ftdi_d2xx/third_party/ftd2xx.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GraniteDevices/SimpleMotionV2/b890f7f99724c66a55915125bb70ddb59ea7e346/drivers/ftdi_d2xx/third_party/ftd2xx.h -------------------------------------------------------------------------------- /drivers/ftdi_d2xx/third_party/linux32/libftd2xx.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GraniteDevices/SimpleMotionV2/b890f7f99724c66a55915125bb70ddb59ea7e346/drivers/ftdi_d2xx/third_party/linux32/libftd2xx.a -------------------------------------------------------------------------------- /drivers/ftdi_d2xx/third_party/linux64/libftd2xx.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GraniteDevices/SimpleMotionV2/b890f7f99724c66a55915125bb70ddb59ea7e346/drivers/ftdi_d2xx/third_party/linux64/libftd2xx.a -------------------------------------------------------------------------------- /drivers/ftdi_d2xx/third_party/osx/libftd2xx.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GraniteDevices/SimpleMotionV2/b890f7f99724c66a55915125bb70ddb59ea7e346/drivers/ftdi_d2xx/third_party/osx/libftd2xx.a -------------------------------------------------------------------------------- /drivers/ftdi_d2xx/third_party/win_32bit/ftd2xx.lib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/GraniteDevices/SimpleMotionV2/b890f7f99724c66a55915125bb70ddb59ea7e346/drivers/ftdi_d2xx/third_party/win_32bit/ftd2xx.lib -------------------------------------------------------------------------------- /drivers/serial/pcserialport.c: -------------------------------------------------------------------------------- 1 | /* 2 | * pcserialport.h 3 | * 4 | * Header for PC serial port access library (win/linux) 5 | * 6 | * Created on: 28.12.2016 7 | * Author: Tero 8 | * 9 | * Inspired by RS232 library by Teunis van Beelen 10 | */ 11 | 12 | 13 | 14 | #include "pcserialport.h" 15 | #include "user_options.h" 16 | #include "simplemotion_private.h" //needed for timeout variable 17 | 18 | #if defined(__unix__) || defined(__APPLE__) 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #if defined(__linux__) 30 | //needed for setting low latency 31 | #include 32 | #endif 33 | 34 | #if defined(__APPLE__) 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #endif 41 | 42 | smBusdevicePointer serialPortOpen(const char * port_device_name, smint32 baudrate_bps, smbool *success) 43 | { 44 | int port_handle; 45 | int err; 46 | int baudrateEnumValue; 47 | struct termios new_port_settings; 48 | int customBaudRate = 0; 49 | *success=smfalse; 50 | 51 | //check if devicename is correct format 52 | if( strncmp(port_device_name,"/dev/tty",8) != 0 && strncmp(port_device_name,"/dev/cu.",8) != 0) 53 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 54 | 55 | port_handle = open(port_device_name, O_RDWR | O_NOCTTY | O_NONBLOCK); 56 | 57 | if(port_handle==-1) 58 | { 59 | smDebug(-1, SMDebugLow, "Serial port error: port open failed"); 60 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 61 | } 62 | 63 | 64 | // open() follows POSIX semantics: multiple open() calls to the same file will succeed 65 | // unless the TIOCEXCL ioctl is issued (except for root) 66 | if (ioctl(port_handle, TIOCEXCL) == -1) { 67 | smDebug(-1, SMDebugLow, "Serial port error: error setting TIOCEXCL"); 68 | close(port_handle); 69 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 70 | } 71 | 72 | // as the port is now open, clear O_NONBLOCK flag for subsequent I/O calls 73 | if (fcntl(port_handle, F_SETFL, 0) == -1) { 74 | smDebug(-1, SMDebugLow, "Serial port error: error clearing O_NONBLOCK"); 75 | close(port_handle); 76 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 77 | } 78 | 79 | switch(baudrate_bps) 80 | { 81 | #if defined(B9600) 82 | case 9600 : baudrateEnumValue = B9600; break; 83 | #endif 84 | #if defined(B19200) 85 | case 19200 : baudrateEnumValue = B19200; break; 86 | #endif 87 | #if defined(B38400) 88 | case 38400 : baudrateEnumValue = B38400; break; 89 | #endif 90 | #if defined(B57600) 91 | case 57600 : baudrateEnumValue = B57600; break; 92 | #endif 93 | #if defined(B115200) 94 | case 115200 : baudrateEnumValue = B115200; break; 95 | #endif 96 | #if defined(B230400) 97 | case 230400 : baudrateEnumValue = B230400; break; 98 | #endif 99 | #if defined(B460800) 100 | case 460800 : baudrateEnumValue = B460800; break; 101 | #endif 102 | #if defined(B500000) 103 | case 500000 : baudrateEnumValue = B500000; break; 104 | #endif 105 | #if defined(B576000) 106 | case 576000 : baudrateEnumValue = B576000; break; 107 | #endif 108 | #if defined(B921600) 109 | case 921600 : baudrateEnumValue = B921600; break; 110 | #endif 111 | #if defined(B1000000) 112 | case 1000000 : baudrateEnumValue = B1000000; break; 113 | #endif 114 | #if defined(B1152000) 115 | case 1115200 : baudrateEnumValue = B1152000; break; 116 | #endif 117 | #if defined(B1500000) 118 | case 1500000 : baudrateEnumValue = B1500000; break; 119 | #endif 120 | #if defined(B2000000) 121 | case 2000000 : baudrateEnumValue = B2000000; break; 122 | #endif 123 | #if defined(B2500000) 124 | case 2500000 : baudrateEnumValue = B2500000; break; 125 | #endif 126 | #if defined(B3000000) 127 | case 3000000 : baudrateEnumValue = B3000000; break; 128 | #endif 129 | #if defined(B3500000) 130 | case 3500000 : baudrateEnumValue = B3500000; break; 131 | #endif 132 | #if defined(B4000000) 133 | case 4000000 : baudrateEnumValue = B4000000; break; 134 | #endif 135 | default: 136 | customBaudRate = 1; 137 | baudrateEnumValue=B9600;//must set something initially, changed later 138 | break; 139 | } 140 | 141 | memset(&new_port_settings, 0, sizeof(new_port_settings)); //reset struct 142 | cfmakeraw(&new_port_settings);//reset struct 143 | new_port_settings.c_cflag = CS8 | CLOCAL | CREAD; 144 | new_port_settings.c_iflag = IGNPAR; 145 | new_port_settings.c_oflag = 0; 146 | new_port_settings.c_lflag = 0; 147 | new_port_settings.c_cc[VMIN] = 0; /* non blocking mode */ 148 | new_port_settings.c_cc[VTIME] = readTimeoutMs/100; /* timeout 100 ms steps */ 149 | if(new_port_settings.c_cc[VTIME]<1)//don't allow value 0ms 150 | new_port_settings.c_cc[VTIME]=1; 151 | #if defined(_BSD_SOURCE) 152 | cfsetspeed(&new_port_settings, baudrateEnumValue); 153 | #else 154 | cfsetispeed(&new_port_settings, baudrateEnumValue); 155 | cfsetospeed(&new_port_settings, baudrateEnumValue); 156 | #endif 157 | 158 | // Activate settings 159 | err = tcsetattr(port_handle, TCSANOW, &new_port_settings); 160 | if(err==-1) 161 | { 162 | close(port_handle); 163 | smDebug(-1, SMDebugLow, "Serial port error: failed to set port parameters"); 164 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 165 | } 166 | 167 | if(customBaudRate) 168 | { 169 | #if defined(IOSSIOSPEED) 170 | speed_t bps = baudrate_bps; 171 | if (ioctl(port_handle, IOSSIOSPEED, &bps) == -1) 172 | { 173 | smDebug(-1, SMDebugLow, "Serial port error: unsupported baudrate\n"); 174 | close(port_handle); 175 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 176 | } 177 | #else 178 | smDebug(-1, SMDebugLow, "Serial port error: unsupported baudrate\n"); 179 | close(port_handle); 180 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 181 | #endif 182 | } 183 | 184 | // set receive latency to 1 ms 185 | #if defined(IOSSDATALAT) 186 | unsigned long microsecs = 1000UL; 187 | if (ioctl(port_handle, IOSSDATALAT, µsecs) == -1) { 188 | smDebug(-1, SMDebugLow, "Serial port error: error setting read latency"); 189 | close(port_handle); 190 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 191 | } 192 | #endif 193 | 194 | #if defined(TIOCGSERIAL) && defined(ASYNC_LOW_LATENCY) 195 | struct serial_struct serial; 196 | if(ioctl(port_handle, TIOCGSERIAL, &serial)!=-1) 197 | { 198 | serial.flags |= ASYNC_LOW_LATENCY; 199 | if(ioctl(port_handle, TIOCSSERIAL, &serial) == -1 ) 200 | { 201 | smDebug(-1, SMDebugLow, "Serial port warning: unable to set low latency mode, maybe try running with root permissions."); 202 | } 203 | } 204 | else 205 | smDebug(-1, SMDebugLow, "Serial port warning: unable to read TIOCGSERIAL for low latency mode, maybe try running with root permissions."); 206 | #endif 207 | 208 | //flush any stray bytes from device receive buffer that may reside in it 209 | //note: according to following page, delay before this may be necessary http://stackoverflow.com/questions/13013387/clearing-the-serial-ports-buffer 210 | usleep(100000); 211 | tcflush(port_handle,TCIOFLUSH); 212 | 213 | *success=smtrue; 214 | return (smBusdevicePointer)port_handle; 215 | } 216 | 217 | 218 | smint32 serialPortRead(smBusdevicePointer busdevicePointer, smuint8 *buf, smint32 size) 219 | { 220 | int serialport_handle=(int)busdevicePointer; 221 | smint32 n; 222 | if(size>4096) size = 4096; 223 | n = read((int)serialport_handle, buf, size); 224 | return n; 225 | } 226 | 227 | 228 | 229 | smint32 serialPortWrite(smBusdevicePointer busdevicePointer, unsigned char *buf, smint32 size) 230 | { 231 | int serialport_handle=(int)busdevicePointer; 232 | return(write((int)serialport_handle, buf, size)); 233 | } 234 | 235 | smbool serialPortMiscOperation(smBusdevicePointer busdevicePointer, BusDeviceMiscOperationType operation) 236 | { 237 | int serialport_handle=(int)busdevicePointer; 238 | switch(operation) 239 | { 240 | case MiscOperationPurgeRX: 241 | //flush any stray bytes from device receive buffer that may reside in it 242 | //note: according to following page, delay before this may be necessary http://stackoverflow.com/questions/13013387/clearing-the-serial-ports-buffer 243 | usleep(20000); 244 | tcflush(serialport_handle,TCIFLUSH); 245 | return smtrue; 246 | break; 247 | case MiscOperationFlushTX://TODO implement 248 | usleep(20000); 249 | //waits until all output written to the object referred to by fd has been transmitted. 250 | tcdrain(serialport_handle); 251 | return smtrue; 252 | break; 253 | default: 254 | smDebug( -1, SMDebugLow, "Serial port error: given MiscOperataion not implemented\n"); 255 | return smfalse; 256 | break; 257 | } 258 | } 259 | 260 | void serialPortClose(smBusdevicePointer busdevicePointer) 261 | { 262 | int serialport_handle=(int)busdevicePointer; 263 | close((int)serialport_handle); 264 | } 265 | 266 | #else //windows: for API, see https://msdn.microsoft.com/en-us/library/ff802693.aspx 267 | 268 | #include 269 | #include 270 | 271 | smBusdevicePointer serialPortOpen(const char *port_device_name, smint32 baudrate_bps, smbool *success) 272 | { 273 | char port_def_string[64], port_name[32]; 274 | HANDLE port_handle; 275 | *success=smfalse; 276 | 277 | //check port name 278 | if(strncmp(port_device_name,"COM",3) != 0 ) 279 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 280 | 281 | sprintf(port_def_string,"baud=%d data=8 parity=N stop=1", (int)baudrate_bps); 282 | sprintf(port_name,"\\\\.\\%s",port_device_name); 283 | 284 | port_handle = CreateFileA(port_name,GENERIC_READ|GENERIC_WRITE,0,NULL,OPEN_EXISTING,0,NULL); 285 | 286 | if(port_handle==INVALID_HANDLE_VALUE) 287 | { 288 | smDebug( -1, SMDebugLow, "Serial port error: Unable to create serial port handle"); 289 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 290 | } 291 | 292 | //fill DCB settings struct 293 | DCB dcb; 294 | FillMemory(&dcb, sizeof(dcb), 0); 295 | dcb.DCBlength = sizeof(dcb); 296 | 297 | if(!BuildCommDCBA(port_def_string, &dcb)) 298 | { 299 | smDebug( -1, SMDebugLow, "Serial port error: Unable to build DCB settings\n"); 300 | CloseHandle(port_handle); 301 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 302 | } 303 | 304 | if(!SetCommState(port_handle, &dcb)) 305 | { 306 | smDebug( -1, SMDebugLow, "Serial port error: Unable to set port settings\n"); 307 | CloseHandle(port_handle); 308 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 309 | } 310 | 311 | //set timeout 312 | COMMTIMEOUTS port_timeouts; 313 | port_timeouts.ReadTotalTimeoutConstant = readTimeoutMs; 314 | port_timeouts.ReadIntervalTimeout = 0; 315 | port_timeouts.ReadTotalTimeoutMultiplier = 0; 316 | port_timeouts.WriteTotalTimeoutMultiplier = 50; 317 | port_timeouts.WriteTotalTimeoutConstant = 50; 318 | 319 | if(!SetCommTimeouts(port_handle, &port_timeouts)) 320 | { 321 | smDebug( -1, SMDebugLow, "Serial port error: Failed to set port timeout settings\n"); 322 | CloseHandle(port_handle); 323 | return(SMBUSDEVICE_RETURN_ON_OPEN_FAIL); 324 | } 325 | 326 | //flush any stray bytes from device receive buffer that may reside in it 327 | PurgeComm((HANDLE)port_handle,PURGE_RXABORT|PURGE_RXCLEAR|PURGE_TXABORT|PURGE_TXCLEAR); 328 | 329 | *success=smtrue; 330 | return( (smBusdevicePointer)port_handle); 331 | } 332 | 333 | 334 | smint32 serialPortRead(smBusdevicePointer busdevicePointer, unsigned char *buf, smint32 size) 335 | { 336 | HANDLE serialport_handle=(HANDLE)busdevicePointer; 337 | smint32 n; 338 | if(size>4096) 339 | size = 4096; 340 | ReadFile((HANDLE)serialport_handle, buf, size, (LPDWORD)((void *)&n), NULL); 341 | return n; 342 | } 343 | 344 | 345 | smint32 serialPortWrite(smBusdevicePointer busdevicePointer, unsigned char *buf, smint32 size) 346 | { 347 | HANDLE serialport_handle=(HANDLE)busdevicePointer; 348 | smint32 n; 349 | if(WriteFile((HANDLE)serialport_handle, buf, size, (LPDWORD)((void *)&n), NULL)) 350 | return n; 351 | return -1; 352 | } 353 | 354 | void serialPortClose(smBusdevicePointer busdevicePointer) 355 | { 356 | HANDLE serialport_handle=(HANDLE)busdevicePointer; 357 | CloseHandle((HANDLE)serialport_handle); 358 | } 359 | 360 | smbool serialPortMiscOperation(smBusdevicePointer busdevicePointer, BusDeviceMiscOperationType operation) 361 | { 362 | HANDLE serialport_handle=(HANDLE)busdevicePointer; 363 | 364 | switch(operation) 365 | { 366 | case MiscOperationPurgeRX: 367 | //flush any stray bytes from device receive buffer that may reside in it 368 | if(PurgeComm((HANDLE)serialport_handle, PURGE_RXABORT|PURGE_RXCLEAR) ) 369 | return smtrue; 370 | else 371 | return smfalse; 372 | break; 373 | case MiscOperationFlushTX: 374 | //flush any stray bytes from device transmit buffer that may reside in it 375 | if(PurgeComm((HANDLE)serialport_handle, PURGE_TXABORT|PURGE_TXCLEAR) ) 376 | return smtrue; 377 | else 378 | return smfalse; 379 | break; 380 | break; 381 | default: 382 | smDebug( -1, SMDebugLow, "Serial port error: given MiscOperataion not implemented\n"); 383 | return smfalse; 384 | break; 385 | } 386 | } 387 | 388 | #endif//windows 389 | -------------------------------------------------------------------------------- /drivers/serial/pcserialport.h: -------------------------------------------------------------------------------- 1 | /* 2 | * pcserialport.h 3 | * 4 | * Header for PC serial port access library (win/linux) 5 | * 6 | * Created on: 28.12.2016 7 | * Author: Tero 8 | * 9 | * Inspired by RS232 library by Teunis van Beelen 10 | */ 11 | 12 | 13 | /* Todo: 14 | -Restore port settings at CloseComport 15 | */ 16 | 17 | #ifndef PCSERAILPORT_H 18 | #define PCSERAILPORT_H 19 | 20 | #include "simplemotion_private.h" 21 | 22 | #ifdef __cplusplus 23 | extern "C" { 24 | #endif 25 | 26 | #include "simplemotion.h" 27 | 28 | smBusdevicePointer serialPortOpen(const char *port_device_name, smint32 baudrate_bps, smbool *success); 29 | smint32 serialPortRead(smBusdevicePointer busdevicePointer, unsigned char *buf, smint32 size); 30 | smint32 serialPortWrite(smBusdevicePointer busdevicePointer, unsigned char *buf, smint32 size); 31 | void serialPortClose(smBusdevicePointer busdevicePointer); 32 | smbool serialPortMiscOperation(smBusdevicePointer busdevicePointer, BusDeviceMiscOperationType operation); 33 | 34 | 35 | #ifdef __cplusplus 36 | } 37 | #endif 38 | 39 | #endif 40 | 41 | 42 | -------------------------------------------------------------------------------- /drivers/tcpip/tcpclient.c: -------------------------------------------------------------------------------- 1 | #include "simplemotion_private.h" 2 | #include "tcpclient.h" 3 | #include "user_options.h" 4 | #include 5 | #include 6 | #include 7 | 8 | #if defined(_WIN32) 9 | #if defined(CM_NONE) 10 | #undef CM_NONE 11 | #endif 12 | #include 13 | #include 14 | #define close closesocket 15 | #define read(SOCKET, BUF, LEN) recv((SOCKET), (BUF), (LEN), 0) 16 | #define write(SOCKET, BUF, LEN) send((SOCKET), (BUF), (LEN), 0) 17 | #ifdef errno 18 | #undef errno 19 | #endif 20 | #define errno (WSAGetLastError()) 21 | #ifdef EINPROGRESS 22 | #undef EINPROGRESS 23 | #endif 24 | #define EINPROGRESS WSAEWOULDBLOCK 25 | #else 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #endif 37 | 38 | #if defined(_WIN32) 39 | static int initwsa() 40 | { 41 | WORD req; 42 | WSADATA data; 43 | req = MAKEWORD(2, 2); 44 | int err = WSAStartup(req, &data); 45 | if (err != 0) 46 | { 47 | printf("WSAStartup failed\n"); 48 | return 0; 49 | } 50 | return 1; 51 | } 52 | #endif 53 | 54 | smBusdevicePointer tcpipPortOpen(const char * devicename, smint32 baudrate_bps, smbool *success) 55 | { 56 | int sockfd; 57 | struct sockaddr_in server; 58 | struct timeval tv; 59 | fd_set myset; 60 | int res, valopt; 61 | socklen_t lon; 62 | unsigned long arg; 63 | 64 | *success=smfalse; 65 | 66 | if (validateIpAddress(devicename, NULL, NULL) != 0) 67 | { 68 | smDebug(-1,SMDebugLow,"TCP/IP: device name '%s' does not appear to be IP address, skipping TCP/IP open attempt (note: this is normal if opening a non-TCP/IP port)\n",devicename); 69 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 70 | } 71 | 72 | char ip_addr[16]; 73 | unsigned short port = 4001; 74 | if (parseIpAddress(devicename, ip_addr, &port) < 0) 75 | { 76 | smDebug(-1,SMDebugLow,"TCP/IP: IP address parse failed\n"); 77 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 78 | } 79 | 80 | 81 | if(baudrate_bps!=SM_BAUDRATE) 82 | { 83 | smDebug(-1,SMDebugLow,"TCP/IP: Non-default baudrate not supported by TCP/IP protocol\n"); 84 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 85 | } 86 | 87 | smDebug(-1,SMDebugLow,"TCP/IP: Attempting to connect to %s:%d\n",ip_addr,port); 88 | 89 | #if defined(_WIN32) 90 | initwsa(); 91 | #endif 92 | 93 | //Create socket 94 | sockfd = socket(AF_INET , SOCK_STREAM , IPPROTO_TCP); 95 | if (sockfd == -1) 96 | { 97 | smDebug(-1,SMDebugLow,"TCP/IP: Socket open failed (sys error: %s)\n",strerror(errno)); 98 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 99 | } 100 | 101 | // Set OFF NAGLE algorithm to disable stack buffering of small packets 102 | int one = 1; 103 | setsockopt(sockfd, IPPROTO_TCP, TCP_NODELAY, (void *)&one, sizeof(one)); 104 | 105 | server.sin_addr.s_addr = inet_addr(ip_addr); 106 | server.sin_family = AF_INET; 107 | server.sin_port = htons(port); 108 | 109 | // Set non-blocking when trying to establish the connection 110 | #if !defined(_WIN32) 111 | arg = fcntl(sockfd, F_GETFL, NULL); 112 | arg |= O_NONBLOCK; 113 | fcntl(sockfd, F_SETFL, arg); 114 | #else 115 | arg = 1; 116 | ioctlsocket(sockfd, FIONBIO, &arg); 117 | #endif 118 | 119 | res = connect(sockfd, (struct sockaddr *)&server, sizeof(server)); 120 | 121 | if (res < 0) //connection not established (at least yet) 122 | { 123 | if (errno == EINPROGRESS) //check if it may be due to non-blocking mode (delayed connect) 124 | { 125 | tv.tv_sec = 5;//max wait time 126 | tv.tv_usec = 0; 127 | FD_ZERO(&myset); 128 | FD_SET((unsigned int)sockfd, &myset); 129 | if (select(sockfd+1, NULL, &myset, NULL, &tv) > 0) 130 | { 131 | lon = sizeof(int); 132 | getsockopt(sockfd, SOL_SOCKET, SO_ERROR, (void*)(&valopt), &lon); 133 | if (valopt) //if valopt!=0, then there was an error. if it's 0, then connection established successfully (will return here from smtrue eventually) 134 | { 135 | smDebug(-1,SMDebugLow,"TCP/IP: Setting socket properties failed (sys error: %s)\n",strerror(errno)); 136 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 137 | } 138 | } 139 | else 140 | { 141 | smDebug(-1,SMDebugLow,"TCP/IP: Setting socket properties failed (sys error: %s)\n",strerror(errno)); 142 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 143 | } 144 | } 145 | else 146 | { 147 | smDebug(-1,SMDebugLow,"TCP/IP: Connecting socket failed (sys error: %s)\n",strerror(errno)); 148 | return SMBUSDEVICE_RETURN_ON_OPEN_FAIL; 149 | } 150 | } 151 | 152 | // Set to blocking mode again 153 | #if !defined(_WIN32) 154 | arg = fcntl(sockfd, F_GETFL, NULL); 155 | arg &= (~O_NONBLOCK); 156 | fcntl(sockfd, F_SETFL, arg); 157 | #else 158 | arg = 0; 159 | ioctlsocket(sockfd, FIONBIO, &arg); 160 | #endif 161 | 162 | *success=smtrue; 163 | return (smBusdevicePointer)sockfd;//compiler warning expected here on 64 bit compilation but it's ok 164 | } 165 | 166 | // Read bytes from socket 167 | int tcpipPortRead(smBusdevicePointer busdevicePointer, unsigned char *buf, int size) 168 | { 169 | int n; 170 | int sockfd=(int)busdevicePointer;//compiler warning expected here on 64 bit compilation but it's ok 171 | fd_set input; 172 | FD_ZERO(&input); 173 | FD_SET((unsigned int)sockfd, &input); 174 | struct timeval timeout; 175 | timeout.tv_sec = readTimeoutMs/1000; 176 | timeout.tv_usec = (readTimeoutMs%1000) * 1000; 177 | 178 | n = select(sockfd + 1, &input, NULL, NULL, &timeout);//n=-1, select failed. 0=no data within timeout ready, >0 data available. Note: sockfd+1 is correct usage. 179 | 180 | if (n < 1) //n=-1 error, n=0 timeout occurred 181 | { 182 | return(-1); 183 | } 184 | if(!FD_ISSET(sockfd, &input)) 185 | { 186 | return(-1);//no data available 187 | } 188 | 189 | n = read(sockfd, (char*)buf, size); 190 | return(n); 191 | } 192 | 193 | int tcpipPortWrite(smBusdevicePointer busdevicePointer, unsigned char *buf, int size) 194 | { 195 | int sockfd=(int)busdevicePointer;//compiler warning expected here on 64 bit compilation but it's ok 196 | int sent = write(sockfd, (char*)buf, size); 197 | if (sent != size) 198 | { 199 | return sent; 200 | } 201 | return sent; 202 | } 203 | 204 | smbool tcpipMiscOperation(smBusdevicePointer busdevicePointer, BusDeviceMiscOperationType operation) 205 | { 206 | switch(operation) 207 | { 208 | case MiscOperationPurgeRX: 209 | { 210 | int n; 211 | do //loop read as long as there is data coming in 212 | { 213 | char discardbuf[256]; 214 | int sockfd=(int)busdevicePointer;//compiler warning expected here on 64 bit compilation but it's ok 215 | fd_set input; 216 | FD_ZERO(&input); 217 | FD_SET((unsigned int)sockfd, &input); 218 | struct timeval timeout; 219 | timeout.tv_sec = 0; 220 | timeout.tv_usec = 0; 221 | 222 | n = select(sockfd + 1, &input, NULL, NULL, &timeout);//Check whether socket is readable. Note: sockfd+1 is correct usage. 223 | 224 | if (n < 0)// n=-1 select error 225 | { 226 | return smfalse;//select failed 227 | } 228 | if (n == 0)// n=0 no data within timeout 229 | { 230 | return smtrue; 231 | } 232 | if(!FD_ISSET(sockfd, &input)) 233 | { 234 | return smtrue; //no data available, redundant check (see above)? 235 | } 236 | //data is available, read it 237 | n = read(sockfd, (char*)discardbuf, 256);//TODO: should we read 1 byte at a time to avoid blocking here? 238 | } while( n>0 ); 239 | return smtrue; 240 | } 241 | break; 242 | case MiscOperationFlushTX: 243 | //FlushTX should be taken care with disabled Nagle algoritmh 244 | return smtrue; 245 | break; 246 | default: 247 | smDebug( -1, SMDebugLow, "TCP/IP: given MiscOperataion not implemented\n"); 248 | return smfalse; 249 | break; 250 | } 251 | } 252 | 253 | void tcpipPortClose(smBusdevicePointer busdevicePointer) 254 | { 255 | int sockfd=(int)busdevicePointer;//compiler warning expected here on 64 bit compilation but it's ok 256 | close(sockfd); 257 | #if defined(_WIN32) 258 | WSACleanup(); 259 | #endif 260 | } 261 | 262 | 263 | //accepted TCP/IP address format is nnn.nnn.nnn.nnn:pppp where n is IP address numbers and p is port number 264 | //params: s=whole ip address with port number with from user, pip_end=output pointer pointint to end of ip address part of s, pport_stasrt pointing to start of port number in s 265 | int validateIpAddress(const char *s, const char **pip_end, 266 | const char **pport_start) 267 | { 268 | int octets = 0; 269 | int ch = 0, prev = 0; 270 | int len = 0; 271 | const char *ip_end = NULL; 272 | const char *port_start = NULL; 273 | 274 | while (*s) 275 | { 276 | ch = *s; 277 | 278 | if (isdigit(ch)) 279 | { 280 | ++len; 281 | // Octet len must be 1-3 digits 282 | if (len > 3) 283 | { 284 | return -1; 285 | } 286 | } 287 | else if (ch == '.' && isdigit(prev)) 288 | { 289 | ++octets; 290 | len = 0; 291 | // No more than 4 octets please 292 | if (octets > 4) 293 | { 294 | return -1; 295 | } 296 | } 297 | else if (ch == ':' && isdigit(prev)) 298 | { 299 | ++octets; 300 | // We want exactly 4 octets at this point 301 | if (octets != 4) 302 | { 303 | return -1; 304 | } 305 | ip_end = s; 306 | ++s; 307 | port_start = s; 308 | while (isdigit((ch = *s))) 309 | ++s; 310 | // After port we want the end of the string 311 | if (ch != '\0') 312 | return -1; 313 | // This will skip over the ++s below 314 | continue; 315 | } 316 | else 317 | { 318 | return -1; 319 | } 320 | 321 | prev = ch; 322 | ++s; 323 | } 324 | 325 | // We reached the end of the string and did not encounter the port 326 | if (*s == '\0' && ip_end == NULL) 327 | { 328 | ++octets; 329 | ip_end = s; 330 | } 331 | 332 | // Check that there are exactly 4 octets 333 | if (octets != 4) 334 | return -1; 335 | 336 | if (pip_end) 337 | *pip_end = ip_end; 338 | 339 | if (pport_start) 340 | *pport_start = port_start; 341 | 342 | return 0; 343 | } 344 | 345 | //params: s=whole ip:port string, ip=output for ip number only, port=output for port number integer 346 | int parseIpAddress(const char *s, char *ip, unsigned short *port) 347 | { 348 | const char *ip_end, *port_start; 349 | 350 | //ip_end and port_start are pointers to memory area of s, not offsets or indexes to s 351 | if (validateIpAddress(s, &ip_end, &port_start) == -1) 352 | return -1; 353 | 354 | int IPAddrLen=ip_end - s; 355 | if(IPAddrLen<7 || IPAddrLen>15 )//check length before writing to *ip 356 | return -1; 357 | 358 | memcpy(ip, s, IPAddrLen); 359 | ip[IPAddrLen] = '\0'; 360 | 361 | if (port_start) 362 | { 363 | *port = 0; 364 | while (*port_start) 365 | { 366 | *port = *port * 10 + (*port_start - '0'); 367 | ++port_start; 368 | } 369 | } 370 | 371 | return 0; 372 | } 373 | 374 | -------------------------------------------------------------------------------- /drivers/tcpip/tcpclient.h: -------------------------------------------------------------------------------- 1 | #ifndef tcpclient_INCLUDED 2 | #define tcpclient_INCLUDED 3 | 4 | #include "simplemotion_private.h" 5 | 6 | #ifdef __cplusplus 7 | extern "C" { 8 | #endif 9 | 10 | //return port handle. sets success=smtrue if ok 11 | smBusdevicePointer tcpipPortOpen(const char * devicename, smint32 baudrate_bps, smbool *success); 12 | int tcpipPortRead(smBusdevicePointer busdevicePointer, unsigned char *, int); 13 | int tcpipPortWrite(smBusdevicePointer busdevicePointer, unsigned char *, int); 14 | smbool tcpipMiscOperation(smBusdevicePointer busdevicePointer, BusDeviceMiscOperationType operation); 15 | void tcpipPortClose(smBusdevicePointer busdevicePointer); 16 | 17 | 18 | //accepted TCP/IP address format is nnn.nnn.nnn.nnn:pppp where n is IP address numbers and p is port number 19 | int validateIpAddress(const char *s, const char **pip_end, 20 | const char **pport_start); 21 | int parseIpAddress(const char *s, char *ip, unsigned short *port); 22 | 23 | 24 | #ifdef __cplusplus 25 | } /* extern "C" */ 26 | #endif 27 | 28 | #endif 29 | 30 | 31 | -------------------------------------------------------------------------------- /makefile.nmake: -------------------------------------------------------------------------------- 1 | CFLAGS = /I. /Iutils /DNOGDI=true /D_CRT_SECURE_NO_WARNINGS /DWIN32_MEAN_AND_LEAN \ 2 | /W4 /NOLOGO 3 | 4 | 5 | OBJS = \ 6 | bufferedmotion.obj \ 7 | busdevice.obj \ 8 | devicedeployment.obj \ 9 | pcserialport.obj \ 10 | tcpclient.obj \ 11 | simplemotion.obj \ 12 | sm_consts.obj 13 | 14 | simplemotionv2.lib: $(OBJS) 15 | if exist simplemotionv2.lib del simplemotionv2.lib 16 | lib /out:simplemotionv2.lib $(OBJS) 17 | 18 | tcpclient.obj: drivers/tcpip/tcpclient.c 19 | cl $(CFLAGS) -c drivers/tcpip/tcpclient.c 20 | 21 | pcserialport.obj: drivers/serial/pcserialport.c 22 | cl $(CFLAGS) -c drivers/serial/pcserialport.c 23 | -------------------------------------------------------------------------------- /simplemotion.h: -------------------------------------------------------------------------------- 1 | //Global SimpleMotion functions & definitions 2 | //Copyright (c) Granite Devices Oy 3 | 4 | #ifndef SIMPLEMOTION_H 5 | #define SIMPLEMOTION_H 6 | 7 | #ifdef WIN32 8 | //dll specs 9 | #ifdef BUILD_DLL 10 | #define LIB __declspec(dllexport) 11 | #else 12 | // #define LIB __declspec(dllimport) 13 | #define LIB 14 | #endif 15 | #else 16 | #define LIB 17 | #endif 18 | 19 | #include 20 | #include 21 | #include "simplemotion_defs.h" 22 | #include "simplemotion_types.h" 23 | 24 | 25 | #ifdef __cplusplus 26 | extern "C"{ 27 | #endif 28 | 29 | 30 | //BusdeviceOpen callback should return this if port open fails (in addition to setting *success to smfalse): 31 | #define SMBUSDEVICE_RETURN_ON_OPEN_FAIL NULL 32 | 33 | 34 | //max number of simultaneously opened buses. change this and recompiple SMlib if 35 | //necessary (to increase channels or reduce to save memory) 36 | //#define SM_MAX_BUSES 5 37 | /////////////////////////////////////////////////////////////////////////////////////// 38 | //FUNCTIONS//////////////////////////////////////////////////////////////////////////// 39 | /////////////////////////////////////////////////////////////////////////////////////// 40 | 41 | /** Open SM RS485 communication bus. Parameters: 42 | -devicename: formatted string that depend on device type to attempt opening. Supported formats/drivers: 43 | --Serial port device: 44 | ---on Windows: COMn where n=port number, i.e. COM2 45 | ---on Linux: /dev/ttyN where N=port name, i.e. /dev/ttyUSB0 or /dev/ttyS0 46 | ---on macOS: /dev/tty.cuN where N=port name 47 | --TCP/IP socket: format is nnn.nnn.nnn.nnn:pppp where n is IP address numbers and p is port number 48 | --FTDI USB serial port (FTDI D2XX API, availablity depends whether library has been compiled with FTDI support enabled, see SimpleMotionV2.pri): 49 | ---Opening by device index: FTDIn where n=index (0 or greater) 50 | ---Opening by device description (programmed in FTDI EEPROM): raw name, i.e. USB-SMV2 or TTL232R (hint: name is displayed in Granity 1.14 or later) 51 | ---Hint: D2XX driver supports listing available devices. See: smGetNumberOfDetectedBuses() and smGetBusDeviceDetails() 52 | -return value: handle to be used with all other commands, -1 if fails 53 | */ 54 | LIB smbus smOpenBus( const char * devicename ); 55 | 56 | /** Same as smOpenBus but with user supplied port driver callbacks */ 57 | LIB smbus smOpenBusWithCallbacks(const char *devicename, BusdeviceOpen busOpenCallback, BusdeviceClose busCloseCallback, BusdeviceReadBuffer busReadCallback, BusdeviceWriteBuffer busWriteCallback , BusdeviceMiscOperation busPurgeCallback); 58 | 59 | /** Change baudrate of SM communication port. This does not affect already opened ports but the next smOpenBus will be opened at the new speed. 60 | Calling this is optional. By default SM bus and all slave devices operates at 460800 BPS speed. 61 | Parameters: 62 | -bps: bus speed in bits per second. for possible choices, see rs232.c (but note that all speeds are not necessarily supported by SM devices) 63 | Typical usage is: 64 | - first call smSetParameter(handle,0,SMP_BUS_SPEED,N) to change speed of all connected slaves to N PBS 65 | - then close port with smCloseBus 66 | - then call smSetBaudrate(N) 67 | - then open bus again with smOpenBus 68 | 69 | The above method does not utilize very useful SM watchog feature that allows securely re-connecting in case of lost connection to the target devices. 70 | For defails, see example at https://granitedevices.com/wiki/Changing_SimpleMotion_baud_rate 71 | */ 72 | LIB void smSetBaudrate( unsigned long pbs ); 73 | 74 | /** Set timeout of how long to wait reply packet from bus. Must be set before smOpenBus and cannot be changed afterwards 75 | * max value 5000ms. Range may depend on underyling OS / drivers. If supplied argument is lower than minimum supported by drivers, 76 | * then driver minimum is used without notice (return SM_OK). 77 | * 78 | * In unix PC serial port minimum is 100ms, on Windows serial port recommended minimum is 30ms and with FTDI driver 10ms. On TCP/IP: TBD. 79 | * 80 | *This is the only function that returns SM_STATUS which doesn't accumulate status bits to be read with getCumulativeStatus because it has no bus handle 81 | */ 82 | LIB SM_STATUS smSetTimeout( smuint16 millsecs ); 83 | 84 | /** Close connection to given bus handle number. This frees communication link therefore makes it available for other apps for opening. 85 | -return value: a SM_STATUS value, i.e. SM_OK if command succeed 86 | */ 87 | LIB SM_STATUS smCloseBus( const smbus bushandle ); 88 | 89 | /** Return SM lib version number in hexadecimal format. 90 | Ie V 2.5.1 would be 0x020501 and 1.2.33 0x010233 */ 91 | LIB smuint32 smGetVersion(); 92 | 93 | 94 | /** Set stream where debug output is written. By default nothing is written. 95 | smVerbosityLevel: 96 | * SMDebugOff=no debug prints (default) 97 | * SMDebugLow=only some excepetion/errors printed 98 | * SMDebugMid=some common function calls printed 99 | * SMDebugHigh=more details of function calls/bus communication printed 100 | * SMDebugTrace=print all raw RX/TX data and parsed read values of RX data 101 | * 102 | * NOTE: for debug prints to work, SM library must be compiled with ENABLE_DEBUG_PRINTS defined (i.e. uncomment 103 | * that definition from simplemotion.h or define it application wide with compiler flag, i.e. -DENABLE_DEBUG_PRINTS). 104 | * Enabling it may slow down & grow binary significantly especially on MCU systems. 105 | */ 106 | LIB void smSetDebugOutput( smVerbosityLevel level, FILE *stream ); 107 | 108 | /** This function returns all occurred SM_STATUS bits after smOpenBus or resetCumulativeStatus call*/ 109 | LIB SM_STATUS getCumulativeStatus( const smbus handle ); 110 | /** Reset cululative status so getCumultiveStatus returns 0 after calling this until one of the other functions are called*/ 111 | LIB SM_STATUS resetCumulativeStatus( const smbus handle ); 112 | 113 | 114 | /** SMV2 Device communication functionss */ 115 | LIB SM_STATUS smAppendCommandToQueue( smbus handle, smuint8 cmdid, smuint16 param ); 116 | LIB SM_STATUS smExecuteCommandQueue( const smbus bushandle, const smaddr targetaddress ); 117 | LIB smuint16 smGetQueuedCommandReturnValue( const smbus bushandle, smuint16 cmdnumber ); 118 | 119 | LIB SM_STATUS smUploadCommandQueueToDeviceBuffer( const smbus bushandle, const smaddr targetaddress ); 120 | LIB SM_STATUS smBytesReceived( const smbus bushandle, smint32 *bytesinbuffer ); 121 | 122 | LIB SM_STATUS smAppendSMCommandToQueue( smbus handle, int smpCmdType, smint32 paramvalue ); 123 | LIB SM_STATUS smGetQueuedSMCommandReturnValue( const smbus bushandle, smint32 *retValue ); 124 | 125 | LIB SM_STATUS smAppendGetParamCommandToQueue( smbus handle, smint16 paramAddress ); 126 | LIB SM_STATUS smGetQueuedGetParamReturnValue( const smbus bushandle, smint32 *retValue ); 127 | LIB SM_STATUS smAppendSetParamCommandToQueue( smbus handle, smint16 paramAddress, smint32 paramValue ); 128 | LIB SM_STATUS smGetQueuedSetParamReturnValue( const smbus bushandle, smint32 *retValue ); 129 | 130 | /** Simple read & write of parameters with internal queueing, so only one call needed. 131 | Use these for non-time critical operations. */ 132 | LIB SM_STATUS smRead1Parameter( const smbus handle, const smaddr nodeAddress, const smint16 paramId1, smint32 *paramVal1 ); 133 | LIB SM_STATUS smRead2Parameters( const smbus handle, const smaddr nodeAddress, const smint16 paramId1, smint32 *paramVal1,const smint16 paramId2, smint32 *paramVal2 ); 134 | LIB SM_STATUS smRead3Parameters( const smbus handle, const smaddr nodeAddress, const smint16 paramId1, smint32 *paramVal1,const smint16 paramId2, smint32 *paramVal2 ,const smint16 paramId3, smint32 *paramVal3 ); 135 | LIB SM_STATUS smSetParameter( const smbus handle, const smaddr nodeAddress, const smint16 paramId, smint32 paramVal ); 136 | 137 | 138 | LIB SM_STATUS smGetBufferClock( const smbus handle, const smaddr targetaddr, smuint16 *clock ); 139 | 140 | /** smFastUpdateCycleWithStructs uses special SimpleMotion command to perform fast turaround communication. May be used with cyclic real time control. 141 | * smFastUpdateCycleWithStructs has been desniged to have lowest possible response time. 142 | * Typically the worst case response is 50 microseconds, which makes it to achieve up to 20kHz call rate. This may be useful especially when using external 143 | * closed loop and controlling motor torque or velocity in real time. 144 | * 145 | * Parameters write and read are unions and contain several bit field arrangements. 146 | * The format mode should be set by setting SMP_FAST_UPDATE_CYCLE_FORMAT value before calling this function. 147 | */ 148 | LIB SM_STATUS smFastUpdateCycleWithStructs( smbus handle, smuint8 nodeAddress, FastUpdateCycleWriteData write, FastUpdateCycleReadData *read); 149 | 150 | 151 | /** smFastUpdateCycle is similar to smFastUpdateCycleWithStructs with raw integer inputs and outputs instead of structures. 152 | * This is deprecated, consider using smFastUpdateCycleWithStructs instead. 153 | */ 154 | LIB SM_STATUS smFastUpdateCycle( smbus handle, smuint8 nodeAddress, smuint16 write1, smuint16 write2, smuint16 *read1, smuint16 *read2); 155 | 156 | /** Return number of bus devices found. details of each device may be consequently fetched by smGetBusDeviceDetails() */ 157 | LIB smint smGetNumberOfDetectedBuses(); 158 | 159 | /** Fetch information of detected bus nodes at certain index. Example: 160 | 161 | smint num=smGetNumberOfDetectedBuses(); 162 | for(int i=0;i 12 | 13 | #define SM_VERSION 0x020700 14 | 15 | //bus device types 16 | #define BUSDEV_NONE 0 17 | #define BUSDEV_RS 1 /* rs232 like com port support */ 18 | #define BUSDEV_FTDI 2 /*not implemented yet: direct FTDI lib support*/ 19 | 20 | #define SM_BUSDEVICENAME_LEN 64 21 | extern unsigned long SMBusBaudrate; //the next opened port (with smOpenBus) will be opened with the PBS defined here (default 460800 BPS) 22 | 23 | //default timeout in ms 24 | //Argon drive's worst case response time should be ~20ms with max length packets 25 | #define SM_READ_TIMEOUT 500 26 | 27 | 28 | extern const smuint8 table_crc16_hi[]; 29 | extern const smuint8 table_crc16_lo[]; 30 | extern const smuint8 table_crc8[]; 31 | extern FILE *smDebugOut; //such as stderr or file handle. if NULL, debug info disbled 32 | extern smuint16 readTimeoutMs; 33 | 34 | #define DEBUG_PRINT_RAW 0x524157 35 | //smDebug: prints debug info to smDebugOut stream. If no handle available, set it to -1, or if wish to print as raw text, set handle to DEBUG_PRINT_RAW. 36 | //set verbositylevel according to frequency of prints made. 37 | //I.e SMDebugLow=low frequency, so it gets displayed when global verbosity level is set to at least Low or set it to Trace which gets filtered 38 | //out if global verbisity level is set less than SMDebugTrace 39 | #ifdef ENABLE_DEBUG_PRINTS 40 | void smDebug( smbus handle, smVerbosityLevel verbositylevel, char *format, ...); 41 | #else 42 | #define smDebug(...) {} 43 | #endif 44 | //accumulates status to internal variable by ORing the bits. returns same value that is fed as paramter 45 | SM_STATUS recordStatus( const smbus handle, const SM_STATUS stat ); 46 | 47 | SM_STATUS smRawCmd( const char *axisname, smuint8 cmd, smuint16 val, smuint32 *retdata ); 48 | 49 | /*Workaround to have packed structs that compile on GCC and MSVC*/ 50 | #ifdef __GNUC__ 51 | #define PACKED __attribute__ ((__packed__)) 52 | #else/*Assuming MSVC*/ 53 | #define PACKED 54 | #pragma pack(push,1) 55 | #endif 56 | 57 | typedef struct { 58 | /* ID=0 param size 30 bits (cmd total 4 bytes) 59 | * ID=1 param size 22 bits (cmd total 3 bytes) 60 | * ID=2 set parameter store address, param: 14 bits=register address (cmd total 2 bytes) 61 | * ID=3 reserved 62 | */ 63 | long param :30; //LSB 30 bits 64 | unsigned long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID 65 | } PACKED SMPayloadCommand32; 66 | 67 | typedef struct { 68 | /* ID=0 param size 30 bits (cmd total 4 bytes) 69 | * ID=1 param size 22 bits (cmd total 3 bytes) 70 | * ID=2 set parameter store address, param: 14 bits=register address (cmd total 2 bytes) 71 | * ID=3 reserved 72 | */ 73 | long param :14; //LSB 30 bits 74 | unsigned long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID 75 | } PACKED SMPayloadCommand16; 76 | 77 | typedef struct { 78 | /* ID=0 param size 30 bits (cmd total 4 bytes) 79 | * ID=1 param size 22 bits (cmd total 3 bytes) 80 | * ID=2 set parameter store address, param: 14 bits=register address (cmd total 2 bytes) 81 | * ID=3 reserved 82 | */ 83 | long param :22; //MSB 30 bits 84 | unsigned long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID 85 | } PACKED SMPayloadCommand24; 86 | 87 | //SM payload command return data structure 88 | typedef struct { 89 | /* ID=0 ret data 30 bits (tot 4 bytes) 90 | * ID=1 ret data 22 bits (tot 3 bytes) 91 | * ID=2 ret data 16 bits (tot 2 bytes), i.e. ACK/NACK. 92 | * ID=3 reserved 93 | */ 94 | long retData: 30; //LSB 30 bits 95 | unsigned long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID 96 | } PACKED SMPayloadCommandRet32; 97 | 98 | //SM payload command return data structure 99 | typedef struct { 100 | /* ID=0 ret data 30 bits (tot 4 bytes) 101 | * ID=1 ret data 22 bits (tot 3 bytes) 102 | * ID=2 ret data 16 bits (tot 2 bytes), i.e. ACK/NACK. 103 | * ID=3 reserved 104 | */ 105 | long retData: 22; //LSB 30 bits 106 | unsigned long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID 107 | } PACKED SMPayloadCommandRet24; 108 | 109 | //SM payload command return data structure 110 | typedef struct { 111 | /* ID=0 ret data 30 bits (tot 4 bytes) 112 | * ID=1 ret data 22 bits (tot 3 bytes) 113 | * ID=2 ret data 16 bits (tot 2 bytes), i.e. ACK/NACK. 114 | * ID=3 reserved 115 | */ 116 | long retData: 14; //LSB 30 bits 117 | unsigned long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID 118 | } PACKED SMPayloadCommandRet16; 119 | 120 | //SM payload command return data structure 121 | typedef struct { 122 | /* ID=0 ret data 30 bits (tot 4 bytes) 123 | * ID=1 ret data 22 bits (tot 3 bytes) 124 | * ID=2 ret data 16 bits (tot 2 bytes), i.e. ACK/NACK. 125 | * ID=3 reserved 126 | */ 127 | long retData: 6; //LSB 30 bits 128 | unsigned long ID:2; //MSB 2 bits. when serailzied to bytestream byte4 must be transmitted first to contain ID 129 | } PACKED SMPayloadCommandRet8; 130 | 131 | typedef union 132 | { 133 | smuint8 U8[4]; 134 | smuint16 U16[2]; 135 | smuint32 U32; 136 | } UnionOf4Bytes; 137 | 138 | /*Workaround to have packed structs that compile on GCC and MSVC*/ 139 | #ifdef __GNUC__ 140 | #else/*Assuming MSVC*/ 141 | #pragma pack(pop) 142 | #undef PACKED 143 | #endif 144 | 145 | 146 | /** Clear pending (stray) bytes in bus device reception buffer and reset receiver state. This may be needed i.e. after restarting device to eliminate clitches that appear in serial line. 147 | -return value: a SM_STATUS value, i.e. SM_OK if command succeed 148 | */ 149 | LIB SM_STATUS smPurge( const smbus bushandle ); 150 | 151 | /** Block until pending TX bytes are phyiscally out. Max blocking time is same that is set with smSetTimeout 152 | -return value: a SM_STATUS value, i.e. SM_OK if command succeed 153 | */ 154 | LIB SM_STATUS smFlushTX( const smbus bushandle ); 155 | 156 | /* OS independent sleep function for SM internal use 157 | * 158 | * SM lib has implementation for unix/win systems (incl linux & mac). For other systems, please add your own smSleepMs implementation in your application. 159 | * i.e. write function in your main.c or any other .c file that get's compiled and linked: 160 | * 161 | * void smSleepMs(int millisecs) 162 | * { 163 | * // do your delay here 164 | * } 165 | */ 166 | void smSleepMs(int millisecs); 167 | 168 | 169 | #endif // SIMPLEMOTION_PRIVATE_H 170 | -------------------------------------------------------------------------------- /simplemotion_types.h: -------------------------------------------------------------------------------- 1 | //Exported type definitions for SM library 2 | //Copyright (c) Granite Devices Oy 3 | 4 | #ifndef SIMPLEMOTION_TYPES_H 5 | #define SIMPLEMOTION_TYPES_H 6 | 7 | #include 8 | 9 | //possible return values (SM_STATUS type) 10 | #define SM_NONE 0 11 | #define SM_OK 1 12 | #define SM_ERR_NODEVICE 2 13 | #define SM_ERR_BUS 4 14 | #define SM_ERR_COMMUNICATION 8 15 | #define SM_ERR_PARAMETER 16 16 | #define SM_ERR_LENGTH 32 17 | 18 | //declare SM lib integer types 19 | typedef long smbus; 20 | typedef uint32_t smuint32; 21 | typedef uint16_t smuint16; 22 | typedef uint8_t smuint8; 23 | typedef int32_t smint32; 24 | typedef int16_t smint16; 25 | typedef int8_t smint8; 26 | typedef int8_t smbool; 27 | typedef smint32 smint; 28 | #define smtrue 1 29 | #define smfalse 0 30 | typedef int SM_STATUS; 31 | typedef smuint8 smaddr; 32 | 33 | // output parameter type of smGetBusDeviceDetails 34 | typedef struct 35 | { 36 | smbool is_simplemotion_device;//smtrue if usable in SM lib 37 | char device_name[64];//name that should be fed to smOpenBus 38 | char description[128];//such as "SimpleMotion USB" 39 | } SM_BUS_DEVICE_INFO; 40 | 41 | /* Parametr type for smSetDebugOutput 42 | * SMDebugOff=no debug prints (default) 43 | * SMDebugLow=only some excepetion/errors printed 44 | * SMDebugMid=some common function calls printed 45 | * SMDebugHigh=more details of function calls/bus communication printed 46 | * SMDebugTrace=print all raw RX/TX data and parsed read values of RX data 47 | */ 48 | typedef enum _smVerbosityLevel {SMDebugOff,SMDebugLow,SMDebugMid,SMDebugHigh,SMDebugTrace} smVerbosityLevel; 49 | 50 | /* Operations for BusdeviceMiscOperation callback. 51 | * 52 | * MiscOperationFlushTX = blocking call to make sure that all data has been physically transmitter 53 | * before returing the function. Max blocking duration is the value set with smSetTimeout. 54 | * If flush operation timeouted, return smfalse (fail), otherwise smtrue (success). 55 | * MiscOperationPurgeRX = discard all incoming data that is waiting to be read. Return smtrue on success, 56 | * smfalse on fail. 57 | * 58 | * If operation is unsupported by the callback, return smfalse. 59 | */ 60 | typedef enum _BusDeviceMiscOperationType {MiscOperationFlushTX,MiscOperationPurgeRX} BusDeviceMiscOperationType; 61 | 62 | //define communication interface device driver callback types 63 | typedef void* smBusdevicePointer; 64 | typedef smBusdevicePointer (*BusdeviceOpen)(const char *port_device_name, smint32 baudrate_bps, smbool *success); 65 | typedef smint32 (*BusdeviceReadBuffer)(smBusdevicePointer busdevicePointer, unsigned char *buf, smint32 size); 66 | typedef smint32 (*BusdeviceWriteBuffer)(smBusdevicePointer busdevicePointer, unsigned char *buf, smint32 size); 67 | typedef smbool (*BusdeviceMiscOperation)(smBusdevicePointer busdevicePointer, BusDeviceMiscOperationType operation ); 68 | typedef void (*BusdeviceClose)(smBusdevicePointer busdevicePointer); 69 | 70 | //must use packed mode for bitfields in structs for smFastUpdateCycleWithStructs 71 | #pragma pack(push,1) 72 | 73 | // input parameter type for smFastUpdateCycleWithStructs 74 | typedef union 75 | { 76 | //raw data 77 | smuint8 U8[4]; 78 | smuint16 U16[2]; 79 | smuint32 U32; 80 | 81 | //use this when SMP_FAST_UPDATE_CYCLE_FORMAT = FAST_UPDATE_CYCLE_FORMAT_DEFAULT 82 | struct 83 | { 84 | smint32 SetPoint:16; 85 | smuint32 _unused:16; 86 | } DEFAULT_Write; 87 | 88 | //use this when SMP_FAST_UPDATE_CYCLE_FORMAT = FAST_UPDATE_CYCLE_FORMAT_ALT1 89 | struct 90 | { 91 | smint32 Setpoint:28; 92 | smuint32 CB1_Enable:1; 93 | smuint32 CB1_ClearFaults:1; 94 | smuint32 CB1_QuickStopSet:1; 95 | smuint32 CB1_BypassTrajPlanner:1; 96 | } ALT1_Write; 97 | 98 | //use this when SMP_FAST_UPDATE_CYCLE_FORMAT = FAST_UPDATE_CYCLE_FORMAT_ALT2 99 | struct 100 | { 101 | smint32 SetpointMainTorque:15; 102 | smint32 SetpointEffectTorque:15; 103 | smuint32 CB1_Enable:1; 104 | smuint32 CB1_Clearfaults:1; 105 | } ALT2_Write; 106 | 107 | //use this when SMP_FAST_UPDATE_CYCLE_FORMAT = FAST_UPDATE_CYCLE_FORMAT_ALT3 108 | struct 109 | { 110 | smint32 SetpointMainTorque:15; 111 | smint32 SetpointEffectTorque:15; 112 | smuint32 CB1_Enable:1; 113 | smuint32 CB1_Clearfaults:1; 114 | } ALT3_Write; 115 | 116 | //use this when SMP_FAST_UPDATE_CYCLE_FORMAT = FAST_UPDATE_CYCLE_FORMAT_ALT4 117 | struct 118 | { 119 | smint32 SetpointMainTorque:15; 120 | smint32 SetpointEffectTorque:15; 121 | smuint32 CB1_Enable:1; 122 | smuint32 CB1_Clearfaults:1; 123 | } ALT4_Write; 124 | } FastUpdateCycleWriteData; 125 | 126 | // output parameter type for smFastUpdateCycleWithStructs 127 | typedef union 128 | { 129 | //raw data 130 | smuint8 U8[4]; 131 | smuint16 U16[2]; 132 | smuint32 U32; 133 | 134 | //use this when SMP_FAST_UPDATE_CYCLE_FORMAT = FAST_UPDATE_CYCLE_FORMAT_DEFAULT 135 | struct 136 | { 137 | smint32 PositionFeedback:16; 138 | smuint32 StatusRegister:16; 139 | } DEFAULT_Read; 140 | 141 | //use this when SMP_FAST_UPDATE_CYCLE_FORMAT = FAST_UPDATE_CYCLE_FORMAT_ALT1 or FAST_UPDATE_CYCLE_FORMAT_ALT2 142 | struct 143 | { 144 | smint32 PositionFeedback:30; 145 | smuint32 Stat_FaultStop:1; 146 | smuint32 Stat_ServoReady:1; 147 | } ALT1_ALT2_Read; 148 | 149 | //use this when SMP_FAST_UPDATE_CYCLE_FORMAT = FAST_UPDATE_CYCLE_FORMAT_ALT1 or FAST_UPDATE_CYCLE_FORMAT_ALT3 150 | struct 151 | { 152 | smint32 PositionFeedback:24; 153 | smuint32 PositionFeedbackSamplingTimestamp:6; //encoder sampling cycle, 400 us periods 154 | smuint32 Stat_FaultStop:1; 155 | smuint32 Stat_ServoReady:1; 156 | } ALT3_Read; 157 | 158 | //use this when SMP_FAST_UPDATE_CYCLE_FORMAT = FAST_UPDATE_CYCLE_FORMAT_ALT1 or FAST_UPDATE_CYCLE_FORMAT_ALT4 159 | struct 160 | { 161 | smuint32 PositionFeedback:24; // scale is full 24 bits per motor revolution. is not hard absolute like it's in ALT3, and will reset to 0 in centering and with offset SMP. 162 | smuint32 PositionFeedbackSamplingTimestamp:6; //encoder sampling cycle, 400 us periods 163 | smuint32 Stat_FaultStop:1; 164 | smuint32 Stat_ServoReady:1; 165 | } ALT4_Read; 166 | } FastUpdateCycleReadData; 167 | 168 | #pragma pack(pop) 169 | 170 | 171 | #endif // SIMPLEMOTION_TYPES_H 172 | -------------------------------------------------------------------------------- /sm485.h: -------------------------------------------------------------------------------- 1 | /* 2 | * SimpleMotion V2 subpacket frame format constants 3 | */ 4 | #ifndef SM485_H 5 | #define SM485_H 6 | 7 | #define SM485_MAX_PAYLOAD_BYTES 120 8 | #define SM485_CRCINIT 0x0 9 | #define SM485_BUFSIZE 128 10 | #define SM485_RSBUFSIZE 128 11 | 12 | //cmd number must be 0-31 13 | 14 | #define SM485_VERSION 90 15 | #define SM485_VERSION_COMPAT 16 | 17 | //00xb 18 | #define SMCMD_MASK_0_PARAMS 0 19 | //01xb, 2 bytes as payload 20 | #define SMCMD_MASK_2_PARAMS 2 21 | //10xb 22 | #define SMCMD_MASK_N_PARAMS 4 23 | //11xb 24 | #define SMCMD_MASK_RESERVED 6 25 | //xx1b 26 | #define SMCMD_MASK_RETURN 1 27 | //110b 28 | #define SMCMD_MASK_PARAMS_BITS 6 29 | 30 | 31 | //cmdnumber must be 0-31 32 | #define SMCMD_ID(cmdnumber, flags) (((cmdnumber)<<3)|flags) 33 | 34 | 35 | //returned subpacket if error occurred. payload is two bytes where second byte is one of SMERR_ values (below) 36 | #define SMCMD_ERROR_RET SMCMD_ID(3,SMCMD_MASK_2_PARAMS|SMCMD_MASK_RETURN) 37 | #define SMERR_CRC 1 38 | #define SMERR_INVALID_CMD 2 39 | #define SMERR_BUF_OVERFLOW 4 40 | #define SMERR_INVALID_PARAMETER 8 41 | #define SMERR_PAYLOAD_SIZE 16 42 | 43 | //format 5, u8 bytesfollowing,u8 toaddr,cmddata,u16 crc 44 | //return, 6, u8 bytesfollowing,u8 fromaddr, returndata,u16 crc 45 | #define SMCMD_INSTANT_CMD SMCMD_ID(4,SMCMD_MASK_N_PARAMS) 46 | #define SMCMD_INSTANT_CMD_RET SMCMD_ID(4,SMCMD_MASK_N_PARAMS|SMCMD_MASK_RETURN) 47 | 48 | //format ID, u8 bytesfollowing,u8 toaddr,cmddata,u16 crc 49 | //return, ID, u8 bytesfollowing,u8 fromaddr,returndata,u16 crc 50 | #define SMCMD_BUFFERED_CMD SMCMD_ID(6,SMCMD_MASK_N_PARAMS) 51 | #define SMCMD_BUFFERED_CMD_RET SMCMD_ID(6,SMCMD_MASK_N_PARAMS|SMCMD_MASK_RETURN) 52 | 53 | //read return data from buffered cmds. repeat this command before SMCMD_BUFFERED_CMD until N!=120 from download 54 | #define SMCMD_BUFFERED_RETURN_DATA SMCMD_ID(7,SMCMD_MASK_0_PARAMS) 55 | #define SMCMD_BUFFERED_RETURN_DATA_RET SMCMD_ID(7,SMCMD_MASK_N_PARAMS|SMCMD_MASK_RETURN) 56 | 57 | //cmd 20,u8 toaddr, u8 crc 58 | //ret 21, u8=2, u8 fromaddr, u16 clock,u16 crc, muut clientit lukee t?n 59 | //oldret 21, u8 fromaddr, u16 clock,u16 crc, muut clientit lukee t?n 60 | #define SMCMD_GET_CLOCK SMCMD_ID(10,SMCMD_MASK_0_PARAMS) 61 | #define SMCMD_GET_CLOCK_RET SMCMD_ID(10,SMCMD_MASK_2_PARAMS|SMCMD_MASK_RETURN) 62 | 63 | // SMCMD_FAST_UPDATE_CYCLE is a high priority command for fast realtime cyclic operation. content and lenght of 64 | // payload data may be application specific and may be controlled by other variables 65 | // cmd u8 ID, u8 toaddr, u32 data u16 crc 66 | // ret u8 ID, u8 fromaddr, u32 data, u16 crc 67 | #define SMCMD_FAST_UPDATE_CYCLE SMCMD_ID(2,SMCMD_MASK_0_PARAMS) 68 | #define SMCMD_FAST_UPDATE_CYCLE_RET SMCMD_ID(2,SMCMD_MASK_0_PARAMS|SMCMD_MASK_RETURN) 69 | 70 | //payload data is sent to the bus unmodified, no other return data 71 | #define SMCMD_ECHO SMCMD_ID(12,SMCMD_MASK_N_PARAMS) 72 | 73 | 74 | #endif 75 | -------------------------------------------------------------------------------- /sm_consts.c: -------------------------------------------------------------------------------- 1 | #include "simplemotion_private.h" 2 | 3 | 4 | /* Table of CRC16 values for high-order byte */ 5 | const smuint8 table_crc16_hi[] = { 6 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 7 | 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 8 | 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 9 | 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 10 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 11 | 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 12 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 13 | 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 14 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 15 | 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 16 | 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 17 | 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 18 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 19 | 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 20 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 21 | 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 22 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 23 | 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 24 | 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 25 | 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 26 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 27 | 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 28 | 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 29 | 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 30 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 31 | 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40 32 | }; 33 | /* Table of CRC16 values for low-order byte */ 34 | const smuint8 table_crc16_lo[] = { 35 | 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 36 | 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 37 | 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 38 | 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 39 | 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 40 | 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, 41 | 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 42 | 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 43 | 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 44 | 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 45 | 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 46 | 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 47 | 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 48 | 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 49 | 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 50 | 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 51 | 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 52 | 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, 53 | 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 54 | 0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 55 | 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 56 | 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 57 | 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 58 | 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, 59 | 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 60 | 0x43, 0x83, 0x41, 0x81, 0x80, 0x40 61 | }; 62 | 63 | //http://www.maxim-ic.com/appnotes.cfm/an_pk/3749 64 | const smuint8 table_crc8[] = { 65 | 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 66 | 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, 67 | 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65, 68 | 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D, 69 | 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5, 70 | 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD, 71 | 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85, 72 | 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD, 73 | 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2, 74 | 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA, 75 | 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2, 76 | 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A, 77 | 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32, 78 | 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A, 79 | 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42, 80 | 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A, 81 | 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C, 82 | 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4, 83 | 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC, 84 | 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4, 85 | 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C, 86 | 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44, 87 | 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C, 88 | 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34, 89 | 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B, 90 | 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63, 91 | 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B, 92 | 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13, 93 | 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB, 94 | 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83, 95 | 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB, 96 | 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3 97 | }; 98 | -------------------------------------------------------------------------------- /tests/Makefile: -------------------------------------------------------------------------------- 1 | # tests/Makefile 2 | # 3 | # Under this directory there will hopefully be more tests in the long run. 4 | # Tests are in the form of standalone executables which can contain multiple 5 | # test cases. Protocol is simple: the test passes if it returns 0 and fails 6 | # otherwise. Usually the failure is caused by the standalone application ending 7 | # up calling abort(3) through a failed assert(3). 8 | # 9 | # To understand the test failure, use of gdb or other debugging tools is adviced. 10 | # 11 | # As such the tests are meant to be run on developer desktops and never 12 | # integrated into any unix-alike operation system environment (mcu, plc, etc). 13 | # 14 | # If you want to run the tests on a platform which does not yet have 15 | # the sanitizer support, just modify this and let us know through the issues! 16 | SANITIZERS = -fsanitize=address -fsanitize=undefined 17 | 18 | CFLAGS = -std=c11 -g -Og -I../ -I../utils $(SANITIZERS) -fstrict-overflow 19 | LIB_CFLAGS = $(CFLAGS) 20 | LDFLAGS = $(SANITIZERS) 21 | 22 | LIB_OUTDIR = ./lib 23 | 24 | .PHONY: clean 25 | 26 | LIB_SOURCES = $(wildcard ../*.c) 27 | LIB_OBJECTS = $(patsubst %.c,$(LIB_OUTDIR)/%.o,$(notdir $(LIB_SOURCES))) 28 | 29 | TEST_CASES_SRC = $(wildcard *.c) 30 | TEST_CASES = $(patsubst %.c,%,$(notdir $(TEST_CASES_SRC))) 31 | 32 | test: $(LIB_OUTDIR) test_all 33 | 34 | test_all: $(TEST_CASES) 35 | @for test in $(TEST_CASES); do retval=0; ./$$test || retval=$?; if [ "$$retval" -ne 0 ]; then echo $$test: failed; exit 1; fi; echo $$test: ok; done 36 | 37 | $(TEST_CASES): %: %.c libsimplemotionv2.a 38 | $(CC) $(CFLAGS) $(LDFLAGS) -o $@ $< libsimplemotionv2.a 39 | 40 | $(LIB_OUTDIR): 41 | mkdir -p $(LIB_OUTDIR) 42 | 43 | libsimplemotionv2.a: $(LIB_OBJECTS) 44 | ar rcs $@ $^ 45 | 46 | $(LIB_OUTDIR)/%.o: ../%.c 47 | $(CC) $(LIB_CFLAGS) -c -o $@ $< 48 | 49 | clean: 50 | rm -f $(OBJ) $(LIB_OBJECTS) $(TEST_CASES) libsimplemotionv2.a 51 | rmdir $(LIB_OUTDIR) 52 | -------------------------------------------------------------------------------- /tests/descriptions.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "../simplemotion.h" 6 | 7 | static int verify_bytes(char* buffer, size_t len, char expected); 8 | static void hexdump(char* buffer, size_t len); 9 | 10 | int main(void) { 11 | const size_t capacity = 1024; 12 | char buffer[capacity]; 13 | // keep filling the buffer with garbage to have strcmp go over the buffer if 14 | // a null byte is missed 15 | memset(buffer, 0xaa, capacity); 16 | 17 | { 18 | assert(smDescribeSmStatus(buffer, 0, SM_NONE) == 4); // return value is how many would had needed to be written (at least) 19 | assert(buffer[0] == 0); // not sure if this is similar to any snprintf operation, but handy 20 | assert(verify_bytes(&buffer[1], capacity - 1, 0xaa) == 0); 21 | } 22 | 23 | { 24 | assert(smDescribeSmStatus(buffer, 3, SM_NONE) == 4); // return value is how many would had needed to be written (at least) 25 | assert(strcmp("NO", buffer) == 0); 26 | assert(verify_bytes(&buffer[3], capacity - 3, 0xaa) == 0); 27 | } 28 | 29 | { 30 | assert(smDescribeSmStatus(buffer, capacity, SM_NONE) == 4); 31 | assert(memcmp("NONE", buffer, 4) == 0); 32 | } 33 | 34 | { 35 | memset(buffer, 0xaa, capacity); 36 | assert(smDescribeSmStatus(buffer, capacity, SM_OK | SM_ERR_NODEVICE | SM_ERR_BUS | SM_ERR_COMMUNICATION | SM_ERR_PARAMETER | SM_ERR_LENGTH | (1 << 18)) > 0); 37 | const char* expected = "OK ERR_NODEVICE ERR_BUS ERR_COMMUNICATION ERR_PARAMETER ERR_LENGTH EXTRA(262144)"; 38 | assert(strcmp(buffer, expected) == 0); 39 | } 40 | 41 | { 42 | memset(buffer, 0xaa, capacity); 43 | assert(smDescribeFault(buffer, capacity, FLT_FOLLOWERROR | FLT_ENCODER | FLT_ALLOC | (1 << 17)) > 0); 44 | const char* expected = "FOLLOWERROR ENCODER PROGRAM_OR_MEM EXTRA(131072)"; 45 | assert(strcmp(buffer, expected) == 0); 46 | } 47 | 48 | { 49 | memset(buffer, 0xaa, capacity); 50 | assert(smDescribeStatus(buffer, capacity, STAT_RUN | STAT_ENABLED | STAT_FAULTSTOP | STAT_SAFE_TORQUE_MODE_ACTIVE | (1 << 19)) > 0); 51 | const char* expected = "RUN ENABLED FAULTSTOP SAFE_TORQUE_MODE_ACTIVE EXTRA(524288)"; 52 | assert(strcmp(buffer, expected) == 0); 53 | } 54 | 55 | { 56 | memset(buffer, 0xaa, capacity); 57 | assert(smDescribeStatus(buffer, capacity, 0) == 0); 58 | assert(buffer[0] == 0); 59 | } 60 | 61 | { 62 | memset(buffer, 0xaa, capacity); 63 | assert(smDescribeFault(buffer, capacity, 0) == 0); 64 | assert(buffer[0] == 0); 65 | } 66 | } 67 | 68 | static void hexdump(char* buffer, size_t len) { 69 | int all_aa = 0; 70 | for (size_t i = 0; i < len; ++i) { 71 | if (i % 16 == 0) { 72 | if (all_aa == 16) { 73 | //break; 74 | } 75 | all_aa = 0; 76 | printf("\n%04lx | ", i); 77 | } 78 | 79 | printf("%02hhx ", buffer[i]); 80 | if ((unsigned char)buffer[i] == 0xaa) { 81 | all_aa++; 82 | } 83 | } 84 | 85 | printf("\n"); 86 | 87 | } 88 | 89 | static int verify_bytes(char* buffer, size_t len, char expected) { 90 | for (size_t i = 0; i < len; ++i) { 91 | if (buffer[i] != expected) { 92 | return i + 1; 93 | } 94 | } 95 | return 0; 96 | } 97 | -------------------------------------------------------------------------------- /user_options.h: -------------------------------------------------------------------------------- 1 | #ifndef USER_OPTIONS_H 2 | #define USER_OPTIONS_H 3 | 4 | /* User modifiable parameters are located in this file AND makefiles (i.e. Qt's SimpleMotionV2.pri or Makefile). 5 | * Be sure to check makefiles for more compile time options. The reason why options are also in makefiles is that 6 | * header alone can't control linking of external libraries. */ 7 | 8 | //comment out to disable, gives smaller & faster code 9 | // Commenting out this will also disable smDescribe* functions. 10 | #define ENABLE_DEBUG_PRINTS 11 | 12 | //max number of simultaneously opened buses. change this and recompiple SMlib if 13 | //necessary (to increase channels or reduce to save memory) 14 | #define SM_MAX_BUSES 10 15 | 16 | 17 | #endif // USER_OPTIONS_H 18 | -------------------------------------------------------------------------------- /utils/crc.c: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * 3 | * Filename: crc.c 4 | * 5 | * Description: Slow and fast implementations of the CRC standards. 6 | * 7 | * Notes: The parameters for each supported CRC standard are 8 | * defined in the header file crc.h. The implementations 9 | * here should stand up to further additions to that list. 10 | * 11 | * 12 | * Copyright (c) 2000 by Michael Barr. This software is placed into 13 | * the public domain and may be used for any purpose. However, this 14 | * notice must not be changed or removed and no warranty is either 15 | * expressed or implied by its publication or distribution. 16 | **********************************************************************/ 17 | 18 | #include "crc.h" 19 | 20 | /* 21 | * Derive parameters from the standard-specific parameters in crc.h. 22 | */ 23 | #define WIDTH (8 * sizeof(crc)) 24 | #define TOPBIT (1 << (WIDTH - 1)) 25 | 26 | #if (REFLECT_DATA == TRUE) 27 | #undef REFLECT_DATA 28 | #define REFLECT_DATA(X) ((unsigned char) reflect((X), 8)) 29 | #else 30 | #undef REFLECT_DATA 31 | #define REFLECT_DATA(X) (X) 32 | #endif 33 | 34 | #if (REFLECT_REMAINDER == TRUE) 35 | #undef REFLECT_REMAINDER 36 | #define REFLECT_REMAINDER(X) ((crc) reflect((X), WIDTH)) 37 | #else 38 | #undef REFLECT_REMAINDER 39 | #define REFLECT_REMAINDER(X) (X) 40 | #endif 41 | 42 | 43 | /********************************************************************* 44 | * 45 | * Function: reflect() 46 | * 47 | * Description: Reorder the bits of a binary sequence, by reflecting 48 | * them about the middle position. 49 | * 50 | * Notes: No checking is done that nBits <= 32. 51 | * 52 | * Returns: The reflection of the original data. 53 | * 54 | *********************************************************************/ 55 | static unsigned long 56 | reflect(unsigned long data, unsigned char nBits) 57 | { 58 | unsigned long reflection = 0x00000000; 59 | unsigned char bit; 60 | 61 | /* 62 | * Reflect the data about the center bit. 63 | */ 64 | for (bit = 0; bit < nBits; ++bit) 65 | { 66 | /* 67 | * If the LSB bit is set, set the reflection of it. 68 | */ 69 | if (data & 0x01) 70 | { 71 | reflection |= (1 << ((nBits - 1) - bit)); 72 | } 73 | 74 | data = (data >> 1); 75 | } 76 | 77 | return (reflection); 78 | 79 | } /* reflect() */ 80 | 81 | 82 | /********************************************************************* 83 | * 84 | * Function: crcSlow() 85 | * 86 | * Description: Compute the CRC of a given message. 87 | * 88 | * Notes: 89 | * 90 | * Returns: The CRC of the message. 91 | * 92 | *********************************************************************/ 93 | crc 94 | crcSlow(unsigned char const message[], int nBytes) 95 | { 96 | crc remainder = INITIAL_REMAINDER; 97 | int byte; 98 | unsigned char bit; 99 | 100 | 101 | /* 102 | * Perform modulo-2 division, a byte at a time. 103 | */ 104 | for (byte = 0; byte < nBytes; ++byte) 105 | { 106 | /* 107 | * Bring the next byte into the remainder. 108 | */ 109 | remainder ^= (REFLECT_DATA(message[byte]) << (WIDTH - 8)); 110 | 111 | /* 112 | * Perform modulo-2 division, a bit at a time. 113 | */ 114 | for (bit = 8; bit > 0; --bit) 115 | { 116 | /* 117 | * Try to divide the current data bit. 118 | */ 119 | if (remainder & TOPBIT) 120 | { 121 | remainder = (remainder << 1) ^ POLYNOMIAL; 122 | } 123 | else 124 | { 125 | remainder = (remainder << 1); 126 | } 127 | } 128 | } 129 | 130 | /* 131 | * The final remainder is the CRC result. 132 | */ 133 | return (REFLECT_REMAINDER(remainder) ^ FINAL_XOR_VALUE); 134 | 135 | } /* crcSlow() */ 136 | 137 | 138 | crc crcTable[256]; 139 | 140 | 141 | /********************************************************************* 142 | * 143 | * Function: crcInit() 144 | * 145 | * Description: Populate the partial CRC lookup table. 146 | * 147 | * Notes: This function must be rerun any time the CRC standard 148 | * is changed. If desired, it can be run "offline" and 149 | * the table results stored in an embedded system's ROM. 150 | * 151 | * Returns: None defined. 152 | * 153 | *********************************************************************/ 154 | void 155 | crcInit(void) 156 | { 157 | crc remainder; 158 | int dividend; 159 | unsigned char bit; 160 | 161 | 162 | /* 163 | * Compute the remainder of each possible dividend. 164 | */ 165 | for (dividend = 0; dividend < 256; ++dividend) 166 | { 167 | /* 168 | * Start with the dividend followed by zeros. 169 | */ 170 | remainder = dividend << (WIDTH - 8); 171 | 172 | /* 173 | * Perform modulo-2 division, a bit at a time. 174 | */ 175 | for (bit = 8; bit > 0; --bit) 176 | { 177 | /* 178 | * Try to divide the current data bit. 179 | */ 180 | if (remainder & TOPBIT) 181 | { 182 | remainder = (remainder << 1) ^ POLYNOMIAL; 183 | } 184 | else 185 | { 186 | remainder = (remainder << 1); 187 | } 188 | } 189 | 190 | /* 191 | * Store the result into the table. 192 | */ 193 | crcTable[dividend] = remainder; 194 | } 195 | 196 | } /* crcInit() */ 197 | 198 | 199 | /********************************************************************* 200 | * 201 | * Function: crcFast() 202 | * 203 | * Description: Compute the CRC of a given message. 204 | * 205 | * Notes: crcInit() must be called first. 206 | * 207 | * Returns: The CRC of the message. 208 | * 209 | *********************************************************************/ 210 | crc 211 | crcFast(unsigned char const message[], int nBytes) 212 | { 213 | crc remainder = INITIAL_REMAINDER; 214 | unsigned char data; 215 | int byte; 216 | 217 | 218 | /* 219 | * Divide the message by the polynomial, a byte at a time. 220 | */ 221 | for (byte = 0; byte < nBytes; ++byte) 222 | { 223 | data = REFLECT_DATA(message[byte]) ^ (remainder >> (WIDTH - 8)); 224 | remainder = crcTable[data] ^ (remainder << 8); 225 | } 226 | 227 | /* 228 | * The final remainder is the CRC. 229 | */ 230 | return (REFLECT_REMAINDER(remainder) ^ FINAL_XOR_VALUE); 231 | 232 | } /* crcFast() */ 233 | 234 | 235 | 236 | /********************************************************************* 237 | * 238 | * Function: byte by byte CRC functions 239 | * 240 | * Description: Compute the CRC of a given message. 241 | * 242 | * Notes: crcInit() must be called first. 243 | * 244 | * Returns: The CRC of the message. 245 | * 246 | *********************************************************************/ 247 | crc bbb_remainder; 248 | void crcFastByteByByteInit() 249 | { 250 | bbb_remainder = INITIAL_REMAINDER; 251 | } 252 | 253 | void crcFastByteByByteFeed(unsigned char const message) 254 | { 255 | unsigned char data = REFLECT_DATA(message) ^ (bbb_remainder >> (WIDTH - 8)); 256 | bbb_remainder = crcTable[data] ^ (bbb_remainder << 8); 257 | } 258 | 259 | crc crcFastByteByByteGetResult() 260 | { 261 | /* 262 | * The final remainder is the CRC. 263 | */ 264 | return (REFLECT_REMAINDER(bbb_remainder) ^ FINAL_XOR_VALUE); 265 | } 266 | 267 | -------------------------------------------------------------------------------- /utils/crc.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | * 3 | * Filename: crc.h 4 | * 5 | * Description: A header file describing the various CRC standards. 6 | * 7 | * Notes: 8 | * 9 | * 10 | * Copyright (c) 2000 by Michael Barr. This software is placed into 11 | * the public domain and may be used for any purpose. However, this 12 | * notice must not be changed or removed and no warranty is either 13 | * expressed or implied by its publication or distribution. 14 | **********************************************************************/ 15 | 16 | #ifndef _crc_h 17 | #define _crc_h 18 | 19 | #include 20 | 21 | #define FALSE 0 22 | #define TRUE !FALSE 23 | 24 | /* 25 | * Select the CRC standard from the list that follows. 26 | */ 27 | #define CRC32 28 | 29 | 30 | #if defined(CRC_CCITT) 31 | 32 | typedef uint16_t crc; 33 | 34 | #define CRC_NAME "CRC-CCITT" 35 | #define POLYNOMIAL 0x1021 36 | #define INITIAL_REMAINDER 0xFFFF 37 | #define FINAL_XOR_VALUE 0x0000 38 | #define REFLECT_DATA FALSE 39 | #define REFLECT_REMAINDER FALSE 40 | #define CHECK_VALUE 0x29B1 41 | 42 | #elif defined(CRC16) 43 | 44 | typedef uint16_t crc; 45 | 46 | #define CRC_NAME "CRC-16" 47 | #define POLYNOMIAL 0x8005 48 | #define INITIAL_REMAINDER 0x0000 49 | #define FINAL_XOR_VALUE 0x0000 50 | #define REFLECT_DATA TRUE 51 | #define REFLECT_REMAINDER TRUE 52 | #define CHECK_VALUE 0xBB3D 53 | 54 | #elif defined(CRC32) 55 | 56 | typedef uint32_t crc; 57 | 58 | #define CRC_NAME "CRC-32" 59 | #define POLYNOMIAL 0x04C11DB7 60 | #define INITIAL_REMAINDER 0xFFFFFFFF 61 | #define FINAL_XOR_VALUE 0xFFFFFFFF 62 | #define REFLECT_DATA TRUE 63 | #define REFLECT_REMAINDER TRUE 64 | #define CHECK_VALUE 0xCBF43926 65 | 66 | #else 67 | 68 | #error "One of CRC_CCITT, CRC16, or CRC32 must be #define'd." 69 | 70 | #endif 71 | 72 | 73 | void crcInit(void); 74 | crc crcSlow(unsigned char const message[], int nBytes); 75 | crc crcFast(unsigned char const message[], int nBytes); 76 | 77 | void crcFastByteByByteInit(); 78 | void crcFastByteByByteFeed(unsigned char const message); 79 | crc crcFastByteByByteGetResult(); 80 | 81 | #endif /* _crc_h */ 82 | --------------------------------------------------------------------------------