diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 9f5164badf..de9e6e4d8c 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -62,9 +62,19 @@ // Add a little extra to account for timing inaccuracy #define TIMEOUT_US 10500 +// If a timeout occurs during serial communication, it will immediately try again this many times +#define TIMEOUT_RETRIES 1 + +// If a timeout occurs while disarmed, it will try again this many times. This should be a higher number, +// because stopping when disarmed is pretty important. +#define STOP_RETRIES 10 + // Number of bytes returned by the Roboclaw when sending command 78, read both encoders #define ENCODER_MESSAGE_SIZE 10 +// Number of bytes for commands 18 and 19, read speeds. +#define ENCODER_SPEED_MESSAGE_SIZE 7 + bool RoboClaw::taskShouldExit = false; RoboClaw::RoboClaw(const char *deviceName): @@ -82,8 +92,6 @@ RoboClaw::RoboClaw(const char *deviceName): _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("RBCLW_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(); @@ -188,7 +196,7 @@ void RoboClaw::taskMain() // If disarmed, I want to be certain that the stop command gets through. int tries = 0; - while (tries < _parameters.stop_retries && ((drive_ret = drive(0.0)) <= 0 || (turn_ret = turn(0.0)) <= 0)) { + while (tries < STOP_RETRIES && ((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); tries++; px4_usleep(TIMEOUT_US); @@ -241,16 +249,22 @@ int RoboClaw::readEncoder() { uint8_t rbuff_pos[ENCODER_MESSAGE_SIZE]; - uint8_t rbuff_speed[11]; + // I am saving space by overlapping the two separate motor speeds, so that the final buffer will look like: + // [ ] + // And I just ignore all of the statuses and checksums. (The _transaction() function internally handles the + // checksum) + uint8_t rbuff_speed[ENCODER_SPEED_MESSAGE_SIZE + 4]; bool success = false; - for (int retry = 0; retry < _parameters.serial_timeout_retries && !success; retry++) { + for (int retry = 0; retry < TIMEOUT_RETRIES && !success; retry++) { success = true; success &= _transaction(CMD_READ_BOTH_ENCODERS, nullptr, 0, &rbuff_pos[0], ENCODER_MESSAGE_SIZE, false, true) == ENCODER_MESSAGE_SIZE; - success &= _transaction(CMD_READ_SPEED_1, nullptr, 0, &rbuff_speed[0], 7, false, true) == 7; - success &= _transaction(CMD_READ_SPEED_2, nullptr, 0, &rbuff_speed[4], 7, false, true) == 7; + success &= _transaction(CMD_READ_SPEED_1, nullptr, 0, &rbuff_speed[0], ENCODER_SPEED_MESSAGE_SIZE, false, + true) == ENCODER_SPEED_MESSAGE_SIZE; + success &= _transaction(CMD_READ_SPEED_2, nullptr, 0, &rbuff_speed[4], ENCODER_SPEED_MESSAGE_SIZE, false, + true) == ENCODER_SPEED_MESSAGE_SIZE; } if (!success) { @@ -548,8 +562,6 @@ 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); diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index 7a28dd45e2..777db210cd 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -178,8 +178,6 @@ private: 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; @@ -188,8 +186,6 @@ private: 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; diff --git a/src/drivers/roboclaw/roboclaw_params.c b/src/drivers/roboclaw/roboclaw_params.c index d33d19eb44..519ef2b538 100644 --- a/src/drivers/roboclaw/roboclaw_params.c +++ b/src/drivers/roboclaw/roboclaw_params.c @@ -75,27 +75,6 @@ PARAM_DEFINE_INT32(RBCLW_READ_PER, 10); */ PARAM_DEFINE_INT32(RBCLW_COUNTS_REV, 1200); -/** - * 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 * @@ -132,4 +111,4 @@ PARAM_DEFINE_INT32(RBCLW_ADDRESS, 128); * @group Roboclaw driver * @reboot_required true */ -PARAM_DEFINE_INT32(RBCLW_BAUD, 8); +PARAM_DEFINE_INT32(RBCLW_BAUD, 1);