diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index c6f5b47952..2edb0dc565 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/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; + } +} diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index 356612930d..67838520c6 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -45,6 +45,7 @@ #include #include +#include #include #include #include @@ -61,7 +62,6 @@ class RoboClaw { public: - static int roboclawTest(int argc, char *argv[]); void taskMain(); static bool taskShouldExit; @@ -86,8 +86,7 @@ public: * @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 @@ private: 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 @@ private: 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); diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp index 513893beda..aa720bccdb 100644 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ b/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; diff --git a/src/drivers/roboclaw/roboclaw_params.c b/src/drivers/roboclaw/roboclaw_params.c new file mode 100644 index 0000000000..46eccfad8b --- /dev/null +++ b/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 + */ + + +/** + * 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);