From d856b15bc75919f73bf8fdf5cd333af4c4f965a7 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Fri, 4 Nov 2016 12:03:07 +0200 Subject: Added new SMP_SYSTEM_CONTROL values and changelog of new SM protocol version 26 --- simplemotion_defs.h | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index f171350..162c03f 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -20,7 +20,7 @@ /* SMV2 protocol change log: * Version 20: * -V20 was introduced with Argon FW 1.0.0 and present at least until 1.4.0 - * Version 21 (is backwards compatible in syntax but not in setpoint behavior, see details below): + * Version 25 (is backwards compatible in syntax but not in setpoint behavior, see details below): * -V25 was introduced with IONI * -setpoint calculation is different: * now there is only one common setpoint and all ABS and INC commands (buffered & instant) @@ -31,6 +31,10 @@ * this makes it possible to insert any parameter read/write commands in middle of buffered motion. * -implemented watchdog functionality in new param SMP_FAULT_BEHAVIOR * -added param SMP_ADDRESS_OFFSET + * Version 26: + * - fast SM command added (actually is also present in late V25 too, but as unofficial feature) + * - watchdog timout now resets bitrate to default and aborts buffered motion + * */ /* Important when using SMV2 protocol: @@ -344,6 +348,11 @@ #define SMP_SYSTEM_CONTROL_MEASURE_MOTOR_RL 256 //resets position mode FB and setpoint values to 0, and also resets homing status. useful after using in vel or torq mode. #define SMP_SYSTEM_CONTROL_RESET_FB_AND_SETPOINT 512 + //writes various FW version specific values into debug parameters + #define SMP_SYSTEM_CONTROL_GET_SPECIAL_DATA 1024 + //write SM bus SM_CRCINIT constant modifier. special purposes only, don't use if unsure because + //it is one time programmable variable (irreversible operation, can't be ever reset to default by provided methods) + #define SMP_SYSTEM_CONTROL_MODIFY_CRCINIT 262144 //follow error tolerance for position control: #define SMP_POS_FERROR_TRIP 555 -- cgit v1.2.3 From 81d0ce7054c930a80d43884eb1ce97112011a1df Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Thu, 17 Nov 2016 21:53:29 +0200 Subject: Added SMP_VEL_D = 203 --- simplemotion_defs.h | 1 + 1 file changed, 1 insertion(+) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index 162c03f..4a12a03 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -456,6 +456,7 @@ #define SMP_VEL_I 200 #define SMP_POS_P 201 #define SMP_VEL_P 202 +#define SMP_VEL_D 203 #define SMP_VEL_FF 220 #define SMP_ACC_FF 221 #define SMP_POS_FF 222 -- cgit v1.2.3 From f2ff42d175b9df9b3242fcb924d640b72dfb91fb Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Thu, 1 Dec 2016 02:10:37 +0200 Subject: Changed FLAG_LIMIT_SWITCHES_NORMALLY_CLOSED_TYPE to FLAG_LIMIT_SWITCHES_NORMALLY_OPEN_TYPE. Also behavior in firmware will change. Originally it was mistakenly specified in wrong polarity because the default choice with value 0 should be closed_type. --- simplemotion_defs.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index 4a12a03..e30b289 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -430,7 +430,7 @@ #define FLAG_INVERTED_HALLS BV(13) #define FLAG_USE_HALLS BV(14) #define FLAG_MECH_BRAKE_DURING_PHASING BV(15) - #define FLAG_LIMIT_SWITCHES_NORMALLY_CLOSED_TYPE BV(16) + #define FLAG_LIMIT_SWITCHES_NORMALLY_OPEN_TYPE BV(16) #define SMP_MOTION_FAULT_THRESHOLD 568 #define SMP_HV_VOLTAGE_HI_LIMIT 569 #define SMP_HV_VOLTAGE_LOW_LIMIT 570 -- cgit v1.2.3 From 1f976e7371a1da4d2b7d3e33587f6c03c33c9eed Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Tue, 20 Dec 2016 13:41:50 +0200 Subject: Added SMP_SYSTEM_CONTROL_CAPTURE_INDEX_POSITION --- simplemotion_defs.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/simplemotion_defs.h b/simplemotion_defs.h index e30b289..ec3e2f7 100644 --- a/simplemotion_defs.h +++ b/simplemotion_defs.h @@ -350,8 +350,10 @@ #define SMP_SYSTEM_CONTROL_RESET_FB_AND_SETPOINT 512 //writes various FW version specific values into debug parameters #define SMP_SYSTEM_CONTROL_GET_SPECIAL_DATA 1024 + //stores encoder index position in SMP_DEBUGPARAM_1. while busy (index not found) SMP_DEBUGPARAM_2 will be 100, after found it is 200. + #define SMP_SYSTEM_CONTROL_CAPTURE_INDEX_POSITION 2048 //write SM bus SM_CRCINIT constant modifier. special purposes only, don't use if unsure because - //it is one time programmable variable (irreversible operation, can't be ever reset to default by provided methods) + //it is one time programmable variable (permanently irreversible operation, can't be ever reset to default by provided methods) #define SMP_SYSTEM_CONTROL_MODIFY_CRCINIT 262144 //follow error tolerance for position control: -- cgit v1.2.3 From 6ce53fb1222a91d3e709e4a604a44a8ca33bc00c Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Thu, 29 Dec 2016 18:36:22 +0200 Subject: Replaced RS232 library by customized code, not tested yet (win or linux) --- SimpleMotionV2.pri | 4 +- busdevice.c | 10 +- pcserialport.c | 209 +++++++++++++++++++++++++ pcserialport.h | 40 +++++ rs232.c | 402 ------------------------------------------------- rs232.h | 81 ---------- simplemotion.c | 5 +- simplemotion_private.h | 4 + 8 files changed, 264 insertions(+), 491 deletions(-) create mode 100644 pcserialport.c create mode 100644 pcserialport.h delete mode 100644 rs232.c delete mode 100644 rs232.h diff --git a/SimpleMotionV2.pri b/SimpleMotionV2.pri index 0885991..29dc8e7 100644 --- a/SimpleMotionV2.pri +++ b/SimpleMotionV2.pri @@ -5,8 +5,8 @@ DEPENDPATH += $$PWD DEFINES += SIMPLEMOTIONV2_LIBRARY -SOURCES += $$PWD/sm_consts.c $$PWD/simplemotion.c $$PWD/busdevice.c $$PWD/rs232.c +SOURCES += $$PWD/sm_consts.c $$PWD/simplemotion.c $$PWD/busdevice.c $$PWD/pcserialport.c HEADERS += $$PWD/simplemotion_private.h\ - $$PWD/rs232.h $$PWD/busdevice.h $$PWD/simplemotion.h $$PWD/sm485.h $$PWD/simplemotion_defs.h + $$PWD/pcserialport.h $$PWD/busdevice.h $$PWD/simplemotion.h $$PWD/sm485.h $$PWD/simplemotion_defs.h diff --git a/busdevice.c b/busdevice.c index c82e27b..c4f89a1 100644 --- a/busdevice.c +++ b/busdevice.c @@ -1,6 +1,6 @@ #include "busdevice.h" -#include "rs232.h" +#include "pcserialport.h" #include #define BD_NONE 0 @@ -67,7 +67,7 @@ smbusdevicehandle smBDOpen( const char *devicename ) if(strncmp(devicename,"COM",3) == 0 || strncmp(devicename,"/dev/tty",8) == 0) //use rs232 lib { - BusDevice[handle].comPort=OpenComport( devicename, SMBusBaudrate ); + BusDevice[handle].comPort=serialPortOpen( devicename, SMBusBaudrate ); if( BusDevice[handle].comPort == -1 ) { return -1; //failed to open @@ -101,7 +101,7 @@ smbool smBDClose( const smbusdevicehandle handle ) if( BusDevice[handle].bdType==BD_RS ) { - CloseComport( BusDevice[handle].comPort ); + serialPortClose( BusDevice[handle].comPort ); BusDevice[handle].opened=smfalse; return smtrue; } @@ -142,7 +142,7 @@ smbool smBDTransmit(const smbusdevicehandle handle) if( BusDevice[handle].bdType==BD_RS ) { - if(SendBuf(BusDevice[handle].comPort,BusDevice[handle].txBuffer, BusDevice[handle].txBufferUsed)==BusDevice[handle].txBufferUsed) + if(serialPortWriteBuffer(BusDevice[handle].comPort,BusDevice[handle].txBuffer, BusDevice[handle].txBufferUsed)==BusDevice[handle].txBufferUsed) { BusDevice[handle].txBufferUsed=0; return smtrue; @@ -166,7 +166,7 @@ smbool smBDRead( const smbusdevicehandle handle, smuint8 *byte ) if( BusDevice[handle].bdType==BD_RS ) { int n; - n=PollComport(BusDevice[handle].comPort, byte, 1); + n=serialPortRead(BusDevice[handle].comPort, byte, 1); if( n!=1 ) return smfalse; else return smtrue; } diff --git a/pcserialport.c b/pcserialport.c new file mode 100644 index 0000000..5abd07e --- /dev/null +++ b/pcserialport.c @@ -0,0 +1,209 @@ +/* + * pcserialport.h + * + * Header for PC serial port access library (win/linux) + * + * Created on: 28.12.2016 + * Author: Tero + * + * Inspired by RS232 library by Teunis van Beelen + */ + + + +#include "pcserialport.h" +#include "simplemotion_private.h" //needed for timeout variable + +#ifdef __linux__ + +#include +#include +#include +#include +#include +#include +#include + +smint32 serialPortOpen(const char * port_device_name, smint32 baudrate_bps) +{ + int port_handle; + int err; + int baudrateEnumValue; + struct termios new_port_settings; + + switch(baudrate_bps) + { + case 9600 : baudrateEnumValue = B9600; break; + case 19200 : baudrateEnumValue = B19200; break; + case 38400 : baudrateEnumValue = B38400; break; + case 57600 : baudrateEnumValue = B57600; break; + case 115200 : baudrateEnumValue = B115200; break; + case 230400 : baudrateEnumValue = B230400; break; + case 460800 : baudrateEnumValue = B460800; break; + case 500000 : baudrateEnumValue = B500000; break; + case 576000 : baudrateEnumValue = B576000; break; + case 921600 : baudrateEnumValue = B921600; break; + case 1000000 : baudrateEnumValue = B1000000; break; + case 1115200 : baudrateEnumValue = B1152000; break; + case 1500000 : baudrateEnumValue = B1500000; break; + case 2000000 : baudrateEnumValue = B2000000; break; + case 2500000 : baudrateEnumValue = B2500000; break; + case 3000000 : baudrateEnumValue = B3000000; break; + case 3500000 : baudrateEnumValue = B3500000; break; + case 4000000 : baudrateEnumValue = B4000000; break; + default : smDebug(-1,Low,"Serial port error: unsupported baudrate\n"); + return(1); + break; + } + + port_handle = open(port_device_name, O_RDWR | O_NOCTTY ); + if(port_handle==-1) + { + smDebug(-1, Low, "Serial port error: port open failed"); + return(port_handle); + } + + memset(&new_port_settings, 0, sizeof(new_port_settings)); //reset struct + + new_port_settings.c_cflag = baudrateEnumValue | CS8 | CLOCAL | CREAD; + new_port_settings.c_iflag = IGNPAR; + new_port_settings.c_oflag = 0; + new_port_settings.c_lflag = 0; + new_port_settings.c_cc[VMIN] = 0; /* non blocking mode */ + new_port_settings.c_cc[VTIME] = readTimeoutMs/100; /* timeout 100 ms steps */ + err = tcsetattr(port_handle, TCSANOW, &new_port_settings); + if(err==-1) + { + close(port_handle); + smDebug(-1, Low, "Serial port error: failed to set port parameters"); + return -1; + } + return port_handle; +} + + +smint32 serialPortRead(smint32 serialport_handle, smuint8 *buf, smint32 size) +{ + smint32 n; + if(size>4096) size = 4096; + n = read(serialport_handle, buf, size); + return n; +} + + +smint32 serialPortWrite(smint32 serialport_handle, unsigned char byte) +{ + smint32 n; + n = write(serialport_handle, &byte, 1); + if(n<0) + return 1; + return 0; +} + + +smint32 serialPortWriteBuffer(smint32 serialport_handle, unsigned char *buf, smint32 size) +{ + return(write(serialport_handle, buf, size)); +} + + +void serialPortClose(smint32 serialport_handle) +{ + close(serialport_handle); +} + +#else //windows: for API, see https://msdn.microsoft.com/en-us/library/ff802693.aspx + +#include +#include + +smint32 serialPortOpen(const char *port_device_name, smint32 baudrate_bps) +{ + char port_def_string[64], port_name[32]; + HANDLE port_handle; + + sprintf(port_def_string,"baud=%d data=8 parity=N stop=1", (int)baudrate_bps); + sprintf(port_name,"\\\\.\\%s",port_device_name); + + port_handle = CreateFileA(port_name,GENERIC_READ|GENERIC_WRITE,0,NULL,OPEN_EXISTING,0,NULL); + + if(port_handle==INVALID_HANDLE_VALUE) + { + smDebug( -1, Low, "Serial port error: Unable to create serial port handle"); + return -1; + } + + //fill DCB settings struct + DCB dcb; + FillMemory(&dcb, sizeof(dcb), 0); + dcb.DCBlength = sizeof(dcb); + + if(!BuildCommDCBA(port_def_string, &dcb)) + { + smDebug( -1, Low, "Serial port error: Unable to build DCB settings\n"); + CloseHandle(port_handle); + return -1; + } + + if(!SetCommState(port_handle, &dcb)) + { + smDebug( -1, Low, "Serial port error: Unable to set port settings\n"); + CloseHandle(port_handle); + return -1; + } + + //set timeout + COMMTIMEOUTS port_timeouts; + port_timeouts.ReadTotalTimeoutConstant = readTimeoutMs; + port_timeouts.ReadIntervalTimeout = 0; + port_timeouts.ReadTotalTimeoutMultiplier = 0; + port_timeouts.WriteTotalTimeoutMultiplier = 50; + port_timeouts.WriteTotalTimeoutConstant = 50; + + if(!SetCommTimeouts(port_handle, &port_timeouts)) + { + smDebug( -1, Low, "Serial port error: Failed to set port timeout settings\n"); + CloseHandle(port_handle); + return(-1); + } + + return( (smint32)port_handle); +} + + +smint32 serialPortRead(smint32 serialport_handle, unsigned char *buf, smint32 size) +{ + smint32 n; + if(size>4096) + size = 4096; + ReadFile((HANDLE)serialport_handle, buf, size, (LPDWORD)((void *)&n), NULL); + return n; +} + + +smint32 serialPortWriteByte(smint32 serialport_handle, unsigned char byte) +{ + smint32 n; + WriteFile((HANDLE)serialport_handle, &byte, 1, (LPDWORD)((void *)&n), NULL); + if(n<0) + return 1; + return 0; +} + + +smint32 serialPortWriteBuffer(smint32 serialport_handle, unsigned char *buf, smint32 size) +{ + smint32 n; + if(WriteFile((HANDLE)serialport_handle, buf, size, (LPDWORD)((void *)&n), NULL)) + return n; + return -1; +} + + +void serialPortClose(smint32 serialport_number) +{ + CloseHandle((HANDLE)serialport_number); +} + + +#endif//windows diff --git a/pcserialport.h b/pcserialport.h new file mode 100644 index 0000000..832786d --- /dev/null +++ b/pcserialport.h @@ -0,0 +1,40 @@ +/* + * pcserialport.h + * + * Header for PC serial port access library (win/linux) + * + * Created on: 28.12.2016 + * Author: Tero + * + * Inspired by RS232 library by Teunis van Beelen + */ + + +/* Todo: + -Restore port settings at CloseComport + */ + +#ifndef PCSERAILPORT_H +#define PCSERAILPORT_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "simplemotion.h" + +//return port handle or -1 if fails +smint32 serialPortOpen(const char *port_device_name, smint32 baudrate_bps); +smint32 serialPortRead(smint32 serialport_handle, unsigned char *buf, smint32 size); +smint32 serialPortWriteByte(smint32 serialport_handle, unsigned char byte); +smint32 serialPortWriteBuffer(smint32 serialport_handle, unsigned char *buf, smint32 size); +void serialPortClose(smint32 serialport_number); + + +#ifdef __cplusplus +} +#endif + +#endif + + diff --git a/rs232.c b/rs232.c deleted file mode 100644 index 4055e2a..0000000 --- a/rs232.c +++ /dev/null @@ -1,402 +0,0 @@ -/* -*************************************************************************** -* -* Author: Teunis van Beelen -* -* Copyright (C) 2005, 2006, 2007, 2008, 2009 Teunis van Beelen -* -* teuniz@gmail.com -* -*************************************************************************** -* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. -* -*************************************************************************** -* -* This version of GPL is at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt -* -*************************************************************************** -*/ - - - -#include "rs232.h" -#include "simplemotion_private.h" - - -#ifdef __linux__ /* Linux */ - - -int OpenComport(const char * comport_name, int baudrate) -{ - int error; - int handle; - int baudr; - struct termios new_port_settings; - - switch(baudrate) - { - case 50 : baudr = B50; - break; - case 75 : baudr = B75; - break; - case 110 : baudr = B110; - break; - case 134 : baudr = B134; - break; - case 150 : baudr = B150; - break; - case 200 : baudr = B200; - break; - case 300 : baudr = B300; - break; - case 600 : baudr = B600; - break; - case 1200 : baudr = B1200; - break; - case 1800 : baudr = B1800; - break; - case 2400 : baudr = B2400; - break; - case 4800 : baudr = B4800; - break; - case 9600 : baudr = B9600; - break; - case 19200 : baudr = B19200; - break; - case 38400 : baudr = B38400; - break; - case 57600 : baudr = B57600; - break; - case 115200 : baudr = B115200; - break; - case 230400 : baudr = B230400; - break; - case 460800 : baudr = B460800; - break; - case 500000 : baudr = B500000; - break; - case 576000 : baudr = B576000; - break; - case 921600 : baudr = B921600; - break; - case 1000000 : baudr = B1000000; - break; - case 1115200 : baudr = B1152000; - break; - case 1500000 : baudr = B1500000; - break; - case 2000000 : baudr = B2000000; - break; - case 2500000 : baudr = B2500000; - break; - case 3000000 : baudr = B3000000; - break; - case 3500000 : baudr = B3500000; - break; - case 4000000 : baudr = B4000000; - break; - default : printf("invalid baudrate\n"); - return(1); - break; - } - - //Cport[comport_number] = open(comports[comport_number], O_RDWR | O_NOCTTY | O_NDELAY); - handle = open(comport_name, O_RDWR | O_NOCTTY ); - if(handle==-1) - { - perror("unable to open comport "); - return(handle); - } - - memset(&new_port_settings, 0, sizeof(new_port_settings)); /* clear the new struct */ - - new_port_settings.c_cflag = baudr | CS8 | CLOCAL | CREAD; - new_port_settings.c_iflag = IGNPAR; - new_port_settings.c_oflag = 0; - new_port_settings.c_lflag = 0; - new_port_settings.c_cc[VMIN] = 0; /* block untill n bytes are received */ - new_port_settings.c_cc[VTIME] = readTimeoutMs/100; /* block untill a timer expires (n * 100 mSec.) */ - error = tcsetattr(handle, TCSANOW, &new_port_settings); - if(error==-1) - { - close(handle); - perror("unable to adjust portsettings "); - return(-1); - } - - return(handle); -} - - -int PollComport(int comport_number, unsigned char *buf, int size) -{ - int n; - - #ifndef __STRICT_ANSI__ /* __STRICT_ANSI__ is defined when the -ansi option is used for gcc */ - if(size>SSIZE_MAX) size = (int)SSIZE_MAX; /* SSIZE_MAX is defined in limits.h */ - #else - if(size>4096) size = 4096; - #endif - - n = read(comport_number, buf, size); - - return(n); -} - - -int SendByte(int comport_number, unsigned char byte) -{ - int n; - - n = write(comport_number, &byte, 1); - if(n<0) return(1); - - return(0); -} - - -int SendBuf(int comport_number, unsigned char *buf, int size) -{ - return(write(comport_number, buf, size)); -} - - -void CloseComport(int comport_number) -{ - close(comport_number); - //feature removed, not restorint old settings : - //tcsetattr(comport_number, TCSANOW, old_port_settings + comport_number); -} - -/* -Constant Description -TIOCM_LE DSR (data set ready/line enable) -TIOCM_DTR DTR (data terminal ready) -TIOCM_RTS RTS (request to send) -TIOCM_ST Secondary TXD (transmit) -TIOCM_SR Secondary RXD (receive) -TIOCM_CTS CTS (clear to send) -TIOCM_CAR DCD (data carrier detect) -TIOCM_CD Synonym for TIOCM_CAR -TIOCM_RNG RNG (ring) -TIOCM_RI Synonym for TIOCM_RNG -TIOCM_DSR DSR (data set ready) -*/ - -int IsCTSEnabled(int comport_number) -{ - int status; - - status = ioctl(comport_number, TIOCMGET, &status); - - if(status&TIOCM_CTS) return(1); - else return(0); -} - - -#else /* windows */ - -#ifdef DEBUG_COMPORT -//for finding problem with windows calls -void print_win32_system_error(char *name) { - // Retrieve, format, and print out a message from the last error. The - // `name' that's passed should be in the form of a present tense noun - // (phrase) such as "opening file". - // - char *ptr = NULL; - FormatMessage( - FORMAT_MESSAGE_ALLOCATE_BUFFER | - FORMAT_MESSAGE_FROM_SYSTEM, - 0, - GetLastError(), - 0, - (char *)&ptr, - 1024, - NULL); - - fprintf(stderr, "\nError %s: %ls\n", name, ptr); - LocalFree(ptr); -} -#endif - -int OpenComport(const char * comport_name, int baudrate) -{ - - char baudr[64], portname[64]; - HANDLE handle; - - - switch(baudrate) - { - case 110 : strcpy(baudr, "baud=110 data=8 parity=N stop=1"); - break; - case 300 : strcpy(baudr, "baud=300 data=8 parity=N stop=1"); - break; - case 600 : strcpy(baudr, "baud=600 data=8 parity=N stop=1"); - break; - case 1200 : strcpy(baudr, "baud=1200 data=8 parity=N stop=1"); - break; - case 2400 : strcpy(baudr, "baud=2400 data=8 parity=N stop=1"); - break; - case 4800 : strcpy(baudr, "baud=4800 data=8 parity=N stop=1"); - break; - case 9600 : strcpy(baudr, "baud=9600 data=8 parity=N stop=1"); - break; - case 19200 : strcpy(baudr, "baud=19200 data=8 parity=N stop=1"); - break; - case 38400 : strcpy(baudr, "baud=38400 data=8 parity=N stop=1"); - break; - case 57600 : strcpy(baudr, "baud=57600 data=8 parity=N stop=1"); - break; - case 115200 : strcpy(baudr, "baud=115200 data=8 parity=N stop=1"); - break; - case 128000 : strcpy(baudr, "baud=128000 data=8 parity=N stop=1"); - break; - case 256000 : strcpy(baudr, "baud=256000 data=8 parity=N stop=1"); - break; - case 460800 : strcpy(baudr, "baud=460800 data=8 parity=N stop=1"); - break; - case 500000 : strcpy(baudr, "baud=500000 data=8 parity=N stop=1"); - break; - case 576000 : strcpy(baudr, "baud=576000 data=8 parity=N stop=1"); - break; - case 921600 : strcpy(baudr, "baud=921600 data=8 parity=N stop=1"); - break; - case 1000000 : strcpy(baudr, "baud=1000000 data=8 parity=N stop=1"); - break; - case 1115200 : strcpy(baudr, "baud=1115200 data=8 parity=N stop=1"); - break; - case 1500000 : strcpy(baudr, "baud=1500000 data=8 parity=N stop=1"); - break; - case 2000000 : strcpy(baudr, "baud=2000000 data=8 parity=N stop=1"); - break; - case 2500000 : strcpy(baudr, "baud=2500000 data=8 parity=N stop=1"); - break; - case 3000000 : strcpy(baudr, "baud=3000000 data=8 parity=N stop=1"); - break; - case 3500000 : strcpy(baudr, "baud=3500000 data=8 parity=N stop=1"); - break; - case 4000000 : strcpy(baudr, "baud=4000000 data=8 parity=N stop=1"); - break; - default : printf("invalid baudrate\n"); - return(-1); - break; - } - - strcpy(portname,"\\\\.\\"); - strcat(portname,comport_name); - - handle = CreateFileA(portname, - GENERIC_READ|GENERIC_WRITE, - 0, /* no share */ - NULL, /* no security */ - OPEN_EXISTING, - 0, /* no threads */ - NULL); /* no templates */ - - if(handle==INVALID_HANDLE_VALUE) - { - printf("unable to open comport\n"); - return(-1); - } - - DCB port_settings; - memset(&port_settings, 0, sizeof(port_settings)); /* clear the new struct */ - port_settings.DCBlength = sizeof(port_settings); - - if(!BuildCommDCBA(baudr, &port_settings)) - { - printf("unable to set comport dcb settings\n"); - CloseHandle(handle); - return(-1); - } - - if(!SetCommState(handle, &port_settings)) - { - printf("unable to set comport cfg settings\n"); - CloseHandle(handle); - return(-1); - } - - COMMTIMEOUTS Cptimeouts; - - Cptimeouts.ReadIntervalTimeout = 0; - Cptimeouts.ReadTotalTimeoutMultiplier = 0; - Cptimeouts.ReadTotalTimeoutConstant = readTimeoutMs; - Cptimeouts.WriteTotalTimeoutMultiplier = 50; - Cptimeouts.WriteTotalTimeoutConstant = 50; - - - if(!SetCommTimeouts(handle, &Cptimeouts)) - { - printf("unable to set comport time-out settings\n"); - CloseHandle(handle); - return(-1); - } - - return( (int)handle); -} - - -int PollComport(int comport_number, unsigned char *buf, int size) -{ - int n; - - if(size>4096) size = 4096; - - /* added the void pointer cast, otherwise gcc will complain about */ - /* "warning: dereferencing type-punned pointer will break strict aliasing rules" */ - - ReadFile((HANDLE)comport_number, buf, size, (LPDWORD)((void *)&n), NULL); - - return(n); -} - - -int SendByte(int comport_number, unsigned char byte) -{ - int n; - - WriteFile((HANDLE)comport_number, &byte, 1, (LPDWORD)((void *)&n), NULL); - - if(n<0) return(1); - - return(0); -} - - - -int SendBuf(int comport_number, unsigned char *buf, int size) -{ - int n; - - if(WriteFile((HANDLE)comport_number, buf, size, (LPDWORD)((void *)&n), NULL)) - { - return(n); - } - - return(-1); -} - - -void CloseComport(int comport_number) -{ - CloseHandle((HANDLE)comport_number); -} - - - -#endif - diff --git a/rs232.h b/rs232.h deleted file mode 100644 index 7c90fea..0000000 --- a/rs232.h +++ /dev/null @@ -1,81 +0,0 @@ -/* -*************************************************************************** -* -* Author: Teunis van Beelen -* -* Copyright (C) 2005, 2006, 2007, 2008, 2009 Teunis van Beelen -* -* teuniz@gmail.com -* -*************************************************************************** -* -* This program is free software; you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation version 2 of the License. -* -* This program is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License along -* with this program; if not, write to the Free Software Foundation, Inc., -* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. -* -*************************************************************************** -* -* This version of GPL is at http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt -* -*************************************************************************** -*/ - -/* libraray modified for Simplemotion, major changes: - -OpenComport now returns actual file handle that must be passed to rx/tx functions, or -1 if fails - - Todo: - -Restore port settings at CloseComport - */ - -#ifndef rs232_INCLUDED -#define rs232_INCLUDED - -#ifdef __cplusplus -extern "C" { -#endif - -#include -#include - - - -#ifdef __linux__ - -#include -#include -#include -#include -#include -#include -#include - -#else - -#include - -#endif - -//return port handle or -1 if fails -int OpenComport(const char * comport_name, int baudrate); -int PollComport(int, unsigned char *, int); -int SendByte(int, unsigned char); -int SendBuf(int, unsigned char *, int); -void CloseComport(int); - - -#ifdef __cplusplus -} /* extern "C" */ -#endif - -#endif - - diff --git a/simplemotion.c b/simplemotion.c index ea773ac..cdee1d1 100644 --- a/simplemotion.c +++ b/simplemotion.c @@ -92,7 +92,10 @@ void smDebug( smbus handle, smVerbosityLevel verbositylevel, char *format, ...) va_start(fmtargs,format); vsnprintf(buffer,sizeof(buffer)-1,format,fmtargs); va_end(fmtargs); - fprintf(smDebugOut,"%s: %s",smBus[handle].busDeviceName, buffer); + if(handle>=0) + fprintf(smDebugOut,"%s: %s",smBus[handle].busDeviceName, buffer); + else + fprintf(smDebugOut,"SMLib: %s",buffer);//no handle given } } #else diff --git a/simplemotion_private.h b/simplemotion_private.h index e46dc1c..85e54f9 100644 --- a/simplemotion_private.h +++ b/simplemotion_private.h @@ -43,6 +43,10 @@ extern const smuint8 table_crc8[]; extern FILE *smDebugOut; //such as stderr or file handle. if NULL, debug info disbled extern smuint16 readTimeoutMs; +//smDebug: prints debug info to smDebugOut stream. If no handle availab, set it to -1. +//set verbositylevel according to frequency of prints made. +//I.e Low=low frequency, so it gets displayed when global verbosity level is set to at least Low or set it to Trace which gets filtered +//out if global verbisity level is set less than Trace void smDebug( smbus handle, smVerbosityLevel verbositylevel, char *format, ...); SM_STATUS smRawCmd( const char *axisname, smuint8 cmd, smuint16 val, smuint32 *retdata ); -- cgit v1.2.3 From b70a617475efe9af38e6d3debc26ff1555714435 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Fri, 27 Jan 2017 14:09:07 +0200 Subject: Make serial port driver unix compliant, not only linux --- pcserialport.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pcserialport.c b/pcserialport.c index 5abd07e..792f079 100644 --- a/pcserialport.c +++ b/pcserialport.c @@ -14,7 +14,7 @@ #include "pcserialport.h" #include "simplemotion_private.h" //needed for timeout variable -#ifdef __linux__ +#ifdef __unix__ #include #include -- cgit v1.2.3 From dcd4a255d2d0650d054aa4fd38d35fdc71126e02 Mon Sep 17 00:00:00 2001 From: Tero Kontkanen Date: Sat, 28 Jan 2017 19:04:37 +0200 Subject: Remove old library test source. Changed version to 0x201000 = 2.1.0 before merging to master. --- simplemotion_private.h | 2 +- smtest.c | 361 ------------------------------------------------- 2 files changed, 1 insertion(+), 362 deletions(-) delete mode 100644 smtest.c diff --git a/simplemotion_private.h b/simplemotion_private.h index 85e54f9..e0f70f3 100644 --- a/simplemotion_private.h +++ b/simplemotion_private.h @@ -19,7 +19,7 @@ #include "busdevice.h" #include -#define SM_VERSION 0x020001 +#define SM_VERSION 0x020100 //max number of simultaneously opened buses. change this and recompiple SMlib if //necessary (to increase channels or reduce to save memory) #define SM_MAX_BUSES 10 diff --git a/smtest.c b/smtest.c deleted file mode 100644 index e021da6..0000000 --- a/smtest.c +++ /dev/null @@ -1,361 +0,0 @@ -#include "simplemotion.h" -#include -#include - -#define ADDR 255 -#include "vsd_cmd.h" - - -//gcc -o testi *.c - -//#include "D:\gd\vsdr\vsdr-stm32\src\simplemotion_defs.h" -#include "/home/tero/shr/svn/granited/firmware_design/eclipseworkspace/vsdr-stm32/src/simplemotion_defs.h" - -smbus h; - smint32 ret; - SM_STATUS stat=0; - - - -void test() -{ - //printf( "stat %d\n", smRawCommand( h, ADDR, 2, 0, &ret ) ); - //printf("ret=%08x\n",ret); - - //printf( "stat %d\n", smRawCommand( h, 2, 2, 0, &ret ) ); - //printf("ret=%08x\n",ret); - - - - //stat|=smAppendSetParamCommandToQueue(h,SMP_DIGITAL_OUT_VALUE_1,151); -// stat|=smAppendSetParamCommandToQueue(h,SMP_INPUT_MULTIPLIER,11); - stat|=smExecuteCommandQueue( h, ADDR ); - stat|=smGetQueuedSetParamReturnValue(h,&ret); - printf("setstat=%d val=%d\n",stat,ret); - - stat=0; - stat|=smAppendGetParamCommandToQueue(h,SMP_BUS_SPEED); - stat|=smAppendGetParamCommandToQueue(h,SMP_SM_VERSION_COMPAT); - stat|=smAppendGetParamCommandToQueue(h,SMP_DIGITAL_OUT_VALUE_1); - stat|=smExecuteCommandQueue( h, ADDR ); - stat|=smGetQueuedGetParamReturnValue(h,&ret); - printf("stat=%d val=%d\n",stat,ret); - stat|=smGetQueuedGetParamReturnValue(h,&ret); - printf("stat=%d val=%d\n",stat,ret); - stat|=smGetQueuedGetParamReturnValue(h,&ret); - printf("stat=%d val=%d\n",stat,ret); - - -/* smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_RETURN_PARAM_LEN ); - smAppendSMCommandToQueue( h, SMPCMD_24B, SMPRET_32B ); - smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_RETURN_PARAM_ADDR ); - smAppendSMCommandToQueue( h, SMPCMD_32B, SMP_BUS_SPEED ); - - smAppendSMCommandToQueue( h, SMPCMD_32B,SMP_INPUT_MULTIPLIER ); - smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_INPUT_MULTIPLIER ); - smAppendSMCommandToQueue( h, SMPCMD_24B, 100 ); -// SMP_INPUT_MULTIPLIER - - - smExecuteCommandQueue( h, ADDR ); - printf("",smGetQueuedSMCommandReturnValue( h )); - printf("",smGetQueuedSMCommandReturnValue( h )); - printf("",smGetQueuedSMCommandReturnValue( h )); - printf("",smGetQueuedSMCommandReturnValue( h )); - printf("",smGetQueuedSMCommandReturnValue( h )); - printf("",smGetQueuedSMCommandReturnValue( h )); - printf("",smGetQueuedSMCommandReturnValue( h )); - // printf("%d\n",smGetQueuedSMCommandReturnValue( h ));*/ - - /* smAppendCommandToQueue( h, CMD_GET_PARAM, RUNTIME_FAULTBITS ); - smAppendCommandToQueue( h, CMD_GET_PARAM, RUNTIME_STATUSBITS ); - smAppendCommandToQueue( h, CMD_GET_PARAM, RUNTIME_FIRMWARE_VERSION ); - smExecuteCommandQueue( h, ADDR ); - printf("%d\n",smGetQueuedCommandReturnValue( h, 0 )); - printf("%d\n",smGetQueuedCommandReturnValue( h, 1 )); - printf("%d\n",smGetQueuedCommandReturnValue( h, 2 )); - */ -} - -void tests(); - -/*BL functions started by writing a value to this param: - * 1:mass erase and reset write pos counter to 0 - * 2:write block - * 3:verify flash (update BOOTLOADER_STAT_FLASH_VERIFIED_OK status) - * 4:start main app - */ -#define SMP_BOOTLOADER_FUNCTION 191 -//upload 16bits of data to buffer. max buffer length 4096 bytes before it must be written with "write block" function -//total uploaded amount must be multiple of 4 bytes before issuing write function -#define SMP_BOOTLOADER_UPLOAD 192 -//bootloaded status -#define SMP_BOOTLOADER_STAT 193 - #define BOOTLOADER_STAT_FLASH_VERIFIED_OK BV(0) -#define SMP_FIRMWARE_VERSION 6010 -#define SMP_BACKWARDS_COMP_VERSION 6012 - -#define SUPPORTED_BL_VERSION_MIN 10000 -#define SUPPORTED_BL_VERSION_MAX 10010 - - -//load app rom from file. args: -//pass pointer to "data". this func will allocate the memory -//return number of 16bit words or -1 if load failed -int loadBLfile( const char *filename, smuint16 **data ) -{ - int bytes; - int i; - FILE *f; - smuint16 *buf; - - //open file - f=fopen(filename,"rb"); - if(f==NULL) - return -1; - - //find file size - fseek(f, 0L, SEEK_END); - bytes = ftell(f); - fseek(f, 0L, SEEK_SET); - printf("size=%d\n",bytes); - //allocate - buf=(smuint16*)malloc((bytes+1)/2);//round upwards - if(buf==NULL) - return -1; - - //load - for(i=0;i=size) - upword=0xeeee; - else - upword=bin[i]; - printf("i=%d\n",i); - - stat|=smAppendSMCommandToQueue( h, SMPCMD_24B, upword ); - i++; - } -// stat|=smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_BOOTLOADER_FUNCTION ); -// stat|=smAppendSMCommandToQueue( h, SMPCMD_24B, 2);//do write - - stat|=smExecuteCommandQueue( h, ADDR ); - - for(c=0;c=size) - upword=0xeeee; - else - upword=bin[i]; - printf("i=%d\n",i); - - stat|=smAppendSMCommandToQueue( h, SMPCMD_24B, upword ); - i++; - } - stat|=smAppendSMCommandToQueue( h, SMPCMD_SETPARAMADDR, SMP_BOOTLOADER_FUNCTION ); - stat|=smAppendSMCommandToQueue( h, SMPCMD_24B, 2);//do write - - stat|=smExecuteCommandQueue( h, ADDR ); - - for(c=0;c