fixes rflex lag issue caused by improper port initialization

Pulled original initialization from player rflex driver code.
Now it is initialized twice - so it needs to be cleaned at some point,
hopefully.

atrvjr_drivers/rflex/include/rflex/rflex-info.h

@@ -0,0 +1,103 @@
+// Original file from player's rflex driver
+#ifndef RFLEX_INFO_H1
+#define RFLEX_INFO_H1
+
+#ifndef TRUE
+#define TRUE  1
+#endif
+
+#ifndef FALSE
+#define FALSE 0
+#endif
+
+#define MAX_NAME_LENGTH                256
+#define MAX_COMMAND_LENGTH             256
+#define MAX_ACMD_SIZE                   48
+#define MAX_NUM_LOOPS                   30
+#define MAX_BUFFER_LENGTH             4096
+
+#define EPSILON                     0.0001
+
+#define TIMEOUT                         -1
+#define WRONG                            0
+#define OK                               1
+
+#define B_STX                         0x02
+#define B_ETX                         0x03
+#define B_ESC                         0x1b
+
+#define STD_TRANS_TORQUE    30000
+#define STD_ROT_ACC         100000
+#define STD_ROT_TORQUE      35000
+
+#define SYS_PORT                 1
+#define MOT_PORT                 2
+#define JSTK_PORT                3
+#define SONAR_PORT               4
+#define DIO_PORT                 5
+#define IR_PORT                  6
+
+#define SYS_LCD_DUMP             0
+#define SYS_STATUS               1
+
+#define MOT_AXIS_GET_SYSTEM      0
+#define MOT_AXIS_GET_MODEL       1
+#define MOT_AXIS_GET_TARGET      2
+#define MOT_AXIS_SET_LIMITS      3
+#define MOT_AXIS_GET_LIMITS      4
+#define MOT_AXIS_SET_POS_LIMITS  5
+#define MOT_AXIS_GET_POS_LIMITS  6
+#define MOT_AXIS_SET_DIR         7
+#define MOT_AXIS_SET_POS         8
+#define MOT_AXIS_GET_MODE        9
+#define MOT_SET_DEFAULTS        10
+#define MOT_BRAKE_SET           11
+#define MOT_BRAKE_RELEASE       12
+#define MOT_SYSTEM_REPORT       33
+#define MOT_SYSTEM_REPORT_REQ   34
+#define MOT_GET_NAXES           65
+#define MOT_SET_GEARING         66
+#define MOT_GET_GEARING         67
+#define MOT_MOTOR_SET_MODE      68
+#define MOT_MOTOR_GET_MODE      69
+#define MOT_MOTOR_SET_PARMS     70
+#define MOT_MOTOR_GET_PARMS     71
+#define MOT_MOTOR_SET_LIMITS    72
+#define MOT_MOTOR_GET_LIMITS    73
+#define MOT_MOTOR_GET_DATA      74
+#define MOT_AXIS_SET_PARMS      75
+#define MOT_AXIS_GET_PARMS      76
+#define MOT_AXIS_SET_PWM_LIMIT  77
+#define MOT_AXIS_GET_PWM_LIMIT  78
+#define MOT_AXIS_SET_PWM        79
+#define MOT_AXIS_GET_PWM        80
+
+#define SONAR_RUN                0
+#define SONAR_GET_UPDATE         1
+#define SONAR_REPORT             2
+
+#define DIO_REPORTS_REQ          0
+#define DIO_REPORT               1
+#define DIO_GET_UPDATE           2
+#define DIO_UPDATE               3
+#define DIO_SET                  4
+
+#define IR_RUN                   0
+#define IR_REPORT                1
+
+#define JSTK_GET_STATE		     0
+
+enum PARITY_TYPE   { N, E, O };
+
+typedef struct {
+  char                       ttyport[MAX_NAME_LENGTH];
+  int                        baud;
+  enum PARITY_TYPE           parity;
+  int                        fd;
+  int                        databits;
+  int                        stopbits;
+  int                        hwf;
+  int                        swf;
+} RFLEX_Device;
+
+#endif

atrvjr_drivers/rflex/include/rflex/rflex-player.hh

@@ -0,0 +1,177 @@
+/*
+ * Functions ripped straight from player's rflex driver to fix the issue
+ * with port initialization.
+ */
+
+#ifndef RFLEX_PLAYER_HH_
+#define RFLEX_PLAYER_HH_
+
+#include <rflex/rflex-info.h>
+#include <fcntl.h>
+#include <termios.h>
+
+// From rflex-io.cc
+int iParity(enum PARITY_TYPE par) {
+	if (par == N)
+		return (IGNPAR);
+	else
+		return (INPCK);
+}
+
+int iSoftControl(int flowcontrol) {
+	if (flowcontrol)
+		return (IXON);
+	else
+		return (IXOFF);
+}
+
+int cDataSize(int numbits) {
+	switch (numbits) {
+	case 5:
+		return (CS5);
+		break;
+	case 6:
+		return (CS6);
+		break;
+	case 7:
+		return (CS7);
+		break;
+	case 8:
+		return (CS8);
+		break;
+	default:
+		return (CS8);
+		break;
+	}
+}
+
+int cStopSize(int numbits) {
+	if (numbits == 2) {
+		return (CSTOPB);
+	} else {
+		return (0);
+	}
+}
+
+int cFlowControl(int flowcontrol) {
+	if (flowcontrol) {
+		return (CRTSCTS);
+	} else {
+		return (CLOCAL);
+	}
+}
+
+int cParity(enum PARITY_TYPE par) {
+	if (par != N) {
+		if (par == O) {
+			return (PARENB | PARODD);
+		} else {
+			return (PARENB);
+		}
+	} else {
+		return (0);
+	}
+}
+
+int cBaudrate(int baudrate) {
+	switch (baudrate) {
+	case 0:
+		return (B0);
+		break;
+	case 300:
+		return (B300);
+		break;
+	case 600:
+		return (B600);
+		break;
+	case 1200:
+		return (B1200);
+		break;
+	case 2400:
+		return (B2400);
+		break;
+	case 4800:
+		return (B4800);
+		break;
+	case 9600:
+		return (B9600);
+		break;
+	case 19200:
+		return (B19200);
+		break;
+	case 38400:
+		return (B38400);
+		break;
+	case 57600:
+		return (B57600);
+		break;
+	case 115200:
+		return (B115200);
+		break;
+#ifdef B230400
+	case 230400:
+		return (B230400);
+		break;
+#endif
+
+	default:
+		return (B9600);
+		break;
+	}
+
+}
+
+void DEVICE_set_params(RFLEX_Device dev) {
+	struct termios ctio;
+
+	tcgetattr(dev.fd, &ctio); /* save current port settings */
+
+	ctio.c_iflag = iSoftControl(dev.swf) | iParity(dev.parity);
+	ctio.c_oflag = 0;
+	ctio.c_cflag = CREAD | cFlowControl(dev.hwf || dev.swf)
+			| cParity(dev.parity) | cDataSize(dev.databits)
+			| cStopSize(dev.stopbits);
+
+	ctio.c_lflag = 0;
+	ctio.c_cc[VTIME] = 0; /* inter-character timer unused */
+	ctio.c_cc[VMIN] = 0; /* blocking read until 0 chars received */
+
+	cfsetispeed(&ctio, (speed_t) cBaudrate(dev.baud));
+	cfsetospeed(&ctio, (speed_t) cBaudrate(dev.baud));
+
+	tcflush(dev.fd, TCIFLUSH);
+	tcsetattr(dev.fd, TCSANOW, &ctio);
+
+}
+
+int DEVICE_connect_port(RFLEX_Device *dev) {
+	if ((dev->fd = open((dev->ttyport), (O_RDWR | O_NOCTTY), 0)) < 0) {
+		return (-1);
+	}
+	DEVICE_set_params(*dev);
+	return (dev->fd);
+}
+// /rflex-io.h
+
+int rflex_open_connection(const char *device_name, int *fd) {
+	RFLEX_Device rdev;
+
+	strncpy(rdev.ttyport, device_name, MAX_NAME_LENGTH);
+	rdev.baud = 115200;
+	rdev.databits = 8;
+	rdev.parity = N;
+	rdev.stopbits = 1;
+	rdev.hwf = 0;
+	rdev.swf = 0;
+
+	printf("trying port %s\n", rdev.ttyport);
+	if (DEVICE_connect_port(&rdev) < 0) {
+		fprintf(stderr, "Can't open device %s\n", rdev.ttyport);
+		return -1;
+	}
+
+	*fd = rdev.fd;
+	return 0;
+}
+
+#endif /* RFLEX_PLAYER_HH_ */

atrvjr_drivers/rflex/src/rflex_driver.cc

@@ -34,6 +34,7 @@
 #include <unistd.h>
 #include <string.h> // memcpy
 #include <fcntl.h>
+#include <rflex/rflex-player.hh>
 
 //finds the sign of a value
 static long sgn( long val ) {
@@ -85,8 +86,12 @@ RFLEX::RFLEX() {
 
 int RFLEX::initialize(const char* device_name) {
     // Open the port
-    fd = open(device_name, O_RDWR | O_NONBLOCK);
-    if (fd == -1) {
+
+	// rflex_open_connection does initialization and the code below duplicates
+	// it. This is not nice and needs to be cleaned when possible.
+	// The function was copied from original player driver to fix control lag
+	// issue we had on ATRVjr.
+    if (rflex_open_connection(device_name, &fd) < 0) {
         fprintf(stderr,"Could not open serial port %s\n", device_name );
         return -1;
     }