Added parameters

src/drivers/roboclaw/RoboClaw.cpp

@@ -62,38 +62,12 @@
 // Add a little extra to account for timing inaccuracy
 #define TIMEOUT_US 10500
 
-// TODO: Make these all parameters
-#define FAILED_TRANSACTION_RETRIES 1
-#define ENCODER_READ_PERIOD_MS 10
-#define ACTUATOR_WRITE_PERIOD_MS 10
-
 // Number of bytes returned by the Roboclaw when sending command 78, read both encoders
 #define ENCODER_MESSAGE_SIZE 10
 
-// TODO: Delete this
-//void printbytes(const char *msg, uint8_t *bytes, int numbytes)
-//{
-//	if (numbytes < 0) {
-//		numbytes = 0;
-//	}
-//
-//	size_t msglen = strlen(msg);
-//	char buff[msglen + (4 * numbytes) + 1];
-//	char *cur = &buff[0];
-//	cur += sprintf(cur, "%s ", msg);
-//
-//	for (int i = 0; i < numbytes; i++) {
-//		cur += sprintf(cur, "0x%02X ", bytes[i]);
-//	}
-//
-//	PX4_INFO("%s", buff);
-//}
-
 bool RoboClaw::taskShouldExit = false;
 
-RoboClaw::RoboClaw(const char *deviceName, uint16_t address, uint16_t pulsesPerRev):
-	_address(address),
-	_pulsesPerRev(pulsesPerRev),
+RoboClaw::RoboClaw(const char *deviceName):
 	_uart(0),
 	_uart_set(),
 	_uart_timeout{.tv_sec = 0, .tv_usec = TIMEOUT_US},
@@ -102,6 +76,17 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address, uint16_t pulsesPerR
 	_motorSpeeds{0, 0}
 
 {
+
+	_param_handles.actuator_write_period_ms = 	param_find("RBCLW_WRITE_PER");
+	_param_handles.encoder_read_period_ms = 	param_find("RBCLW_READ_PER");
+	_param_handles.counts_per_rev = 			param_find("RBCLW_COUNTS_REV");
+	_param_handles.serial_baud_rate = 			param_find("RMCLW_BAUD");
+	_param_handles.serial_timeout_retries = 	param_find("RBCLW_RETRIES");
+	_param_handles.stop_retries = 				param_find("RBCLW_STOP_RETRY");
+	_param_handles.address = 					param_find("RBCLW_ADDRESS");
+
+	_parameters_update();
+
 	// start serial port
 	_uart = open(deviceName, O_RDWR | O_NOCTTY);
 
@@ -114,11 +99,11 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address, uint16_t pulsesPerR
 	if (ret < 0) { err(1, "failed to get attr"); }
 
 	uart_config.c_oflag &= ~ONLCR; // no CR for every LF
-	ret = cfsetispeed(&uart_config, B38400);
+	ret = cfsetispeed(&uart_config, _parameters.serial_baud_rate);
 
 	if (ret < 0) { err(1, "failed to set input speed"); }
 
-	ret = cfsetospeed(&uart_config, B38400);
+	ret = cfsetospeed(&uart_config, _parameters.serial_baud_rate);
 
 	if (ret < 0) { err(1, "failed to set output speed"); }
 
@@ -154,7 +139,7 @@ void RoboClaw::taskMain()
 	int waitTime = 0;
 
 	_actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0));
-	orb_set_interval(_actuatorsSub, ACTUATOR_WRITE_PERIOD_MS);
+	orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms);
 
 	_armedSub = orb_subscribe(ORB_ID(actuator_armed));
 
@@ -186,7 +171,7 @@ void RoboClaw::taskMain()
 			if (disarmed) {
 				// If disarmed, I want to be certain that the stop command gets through.
 				while ((drive_ret = drive(0.0)) <= 0 || (turn_ret = turn(0.0)) <= 0) {
-					PX4_ERR("Error trying to stop: Drive: %d, Turn: %d", drive_ret, turn_ret);
+					//PX4_ERR("Error trying to stop: Drive: %d, Turn: %d", drive_ret, turn_ret);
 					px4_usleep(TIMEOUT_US);
 				}
 
@@ -218,7 +203,7 @@ void RoboClaw::taskMain()
 			}
 		}
 
-		waitTime = ENCODER_READ_PERIOD_MS * 1000 - (hrt_absolute_time() - encoderTaskLastRun);
+		waitTime = _parameters.encoder_read_period_ms * 1000 - (hrt_absolute_time() - encoderTaskLastRun);
 		waitTime = waitTime < 0 ? 0 : waitTime;
 	}
 
@@ -233,7 +218,7 @@ int RoboClaw::readEncoder()
 	uint8_t rbuff[ENCODER_MESSAGE_SIZE];
 	int nread = 0;
 
-	for (int retry = 0; retry < FAILED_TRANSACTION_RETRIES && nread == 0; retry++) {
+	for (int retry = 0; retry < _parameters.serial_timeout_retries && nread == 0; retry++) {
 		nread =  _transaction(CMD_READ_BOTH_ENCODERS, nullptr, 0, &rbuff[0], ENCODER_MESSAGE_SIZE, false, true);
 	}
 
@@ -441,7 +426,7 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
 
 	tcflush(_uart, TCIOFLUSH); // flush  buffers
 	uint8_t buf[wbytes + 4];
-	buf[0] = _address;
+	buf[0] = (uint8_t) _parameters.address;
 	buf[1] = cmd;
 
 	if (wbuff) {
@@ -525,3 +510,53 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
 		}
 	}
 }
+
+void RoboClaw::_parameters_update()
+{
+	param_get(_param_handles.serial_timeout_retries, &_parameters.serial_timeout_retries);
+	param_get(_param_handles.stop_retries, &_parameters.stop_retries);
+	param_get(_param_handles.counts_per_rev, &_parameters.counts_per_rev);
+	param_get(_param_handles.encoder_read_period_ms, &_parameters.encoder_read_period_ms);
+	param_get(_param_handles.actuator_write_period_ms, &_parameters.actuator_write_period_ms);
+	param_get(_param_handles.address, &_parameters.address);
+
+	int baudRate;
+	param_get(_param_handles.serial_baud_rate, &baudRate);
+
+	switch (baudRate) {
+	case 1:
+		_parameters.serial_baud_rate = B2400;
+		break;
+
+	case 2:
+		_parameters.serial_baud_rate = B9600;
+		break;
+
+	case 3:
+		_parameters.serial_baud_rate = B19200;
+		break;
+
+	case 4:
+		_parameters.serial_baud_rate = B38400;
+		break;
+
+	case 5:
+		_parameters.serial_baud_rate = B57600;
+		break;
+
+	case 6:
+		_parameters.serial_baud_rate = B115200;
+		break;
+
+	case 7:
+		_parameters.serial_baud_rate = B230400;
+		break;
+
+	case 8:
+		_parameters.serial_baud_rate = B460800;
+		break;
+
+	default:
+		_parameters.serial_baud_rate = B9600;
+	}
+}

src/drivers/roboclaw/RoboClaw.hpp

@@ -45,6 +45,7 @@
 
 #include <poll.h>
 #include <stdio.h>
+#include <termios.h>
 #include <uORB/SubscriptionPollable.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/wheel_encoders.h>
@@ -61,7 +62,6 @@ class RoboClaw
 {
 public:
 
-	static int roboclawTest(int argc, char *argv[]);
 	void taskMain();
 	static bool taskShouldExit;
 
@@ -86,8 +86,7 @@ class RoboClaw
 	 * @param pulsesPerRev # of encoder
 	 *  pulses per revolution of wheel
 	 */
-	RoboClaw(const char *deviceName, uint16_t address,
-		 uint16_t pulsesPerRev);
+	RoboClaw(const char *deviceName);
 
 	/**
 	 * deconstructor
@@ -185,8 +184,25 @@ class RoboClaw
 		CMD_SIGNED_DUTYCYCLE_2 = 33,
 	};
 
-	uint8_t _address;
-	uint16_t _pulsesPerRev;
+	struct {
+		speed_t serial_baud_rate;
+		int32_t counts_per_rev;
+		int32_t serial_timeout_retries;
+		int32_t stop_retries;
+		int32_t encoder_read_period_ms;
+		int32_t actuator_write_period_ms;
+		int32_t address;
+	} _parameters{};
+
+	struct {
+		param_t serial_baud_rate;
+		param_t counts_per_rev;
+		param_t serial_timeout_retries;
+		param_t stop_retries;
+		param_t encoder_read_period_ms;
+		param_t actuator_write_period_ms;
+		param_t address;
+	} _param_handles{};
 
 	int _uart;
 	fd_set _uart_set;
@@ -206,6 +222,8 @@ class RoboClaw
 	int64_t _encoderCounts[2];
 	int32_t _motorSpeeds[2];
 
+	void _parameters_update();
+
 	static uint16_t _calcCRC(const uint8_t *buf, size_t n, uint16_t init = 0);
 	int _sendUnsigned7Bit(e_command command, float data);
 	int _sendSigned16Bit(e_command command, float data);

src/drivers/roboclaw/roboclaw_main.cpp

@@ -153,7 +153,7 @@ int roboclaw_thread_main(int argc, char *argv[])
 	       deviceName, address, pulsesPerRev);
 
 	// start
-	RoboClaw roboclaw(deviceName, address, pulsesPerRev);
+	RoboClaw roboclaw(deviceName);
 
 	thread_running = true;
 

src/drivers/roboclaw/roboclaw_params.c

@@ -0,0 +1,134 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file roboclaw_params.c
+ *
+ * Parameters defined by the Roboclaw driver.
+ *
+ * The Roboclaw will need to be configured to match these parameters. For information about configuring the
+ * Roboclaw, see http://downloads.ionmc.com/docs/roboclaw_user_manual.pdf
+ *
+ * @author Timothy Scott <timothy@auterion.com>
+ */
+
+
+/**
+ * Uart write period
+ *
+ * How long to wait, in Milliseconds, between writing actuator controls over Uart to the Roboclaw
+ * @unit ms
+ * @min 1
+ * @max 1000
+ * @group Roboclaw driver
+ */
+PARAM_DEFINE_INT32(RBCLW_WRITE_PER, 10);
+
+/**
+ * Encoder read period
+ *
+ * How long to wait, in Milliseconds, between reading wheel encoder values over Uart from the Roboclaw
+ * @unit ms
+ * @min 1
+ * @max 1000
+ * @group Roboclaw driver
+ */
+PARAM_DEFINE_INT32(RBCLW_READ_PER, 10);
+
+/**
+ * Encoder counts per revolution
+ *
+ * Number of encoder counts for one revolution. The roboclaw treats analog encoders (potentiometers) as having 2047
+ * counts per rev. The default value of 1200 corresponds to the default configuration of the Aion R1 rover.
+ * @min 1
+ * @group Roboclaw driver
+ */
+PARAM_DEFINE_INT32(RBCLW_COUNTS_REV, 1200);
+
+/**
+ * Roboclaw serial baud rate
+ *
+ * Baud rate of the serial communication with the Roboclaw. The Roboclaw must be configured to match this rate.
+ * @min 1
+ * @max 8
+ * @value 1 2400 baud
+ * @value 2 9600 baud
+ * @value 3 19200 baud
+ * @value 4 38400 baud
+ * @value 5 57600 baud
+ * @value 6 115200 baud
+ * @value 7 230400 baud
+ * @value 8 460800 baud
+ * @group Roboclaw driver
+ */
+PARAM_DEFINE_INT32(RBCLW_BAUD, 8);
+
+/**
+ * Communication retries
+ *
+ * If communication ever fails with the Roboclaw, it will be immediately retried, up to RBCLW_RETRIES times in total.
+ * @min 1
+ * @max 10
+ * @group Roboclaw driver
+ */
+PARAM_DEFINE_INT32(RBCLW_RETRIES, 1);
+
+/**
+ * Stop retries
+ *
+ * When disarmed, if communication is interrupted with the Roboclaw, it will continue to try to stop up to
+ * this many times.
+ * @min 1
+ * @max 100
+ * @group Roboclaw driver
+ */
+PARAM_DEFINE_INT32(RBCLW_STOP_RETRY, 10);
+
+/**
+ * Address of the Roboclaw
+ *
+ * The Roboclaw can be configured to have an address from 0x80 to 0x87, inclusive. It must be configured to match
+ * this parameter.
+ * @min 128
+ * @max 135
+ * @value 128 0x80
+ * @value 129 0x81
+ * @value 130 0x82
+ * @value 131 0x83
+ * @value 132 0x84
+ * @value 133 0x85
+ * @value 134 0x86
+ * @value 135 0x87
+ * @group Roboclaw driver
+ */
+PARAM_DEFINE_INT32(RBCLW_ADDRESS, 128);