From f5489b03dd9846b24e91272e93aa27b6d0b9613d Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Wed, 4 Oct 2023 17:44:28 +0200 Subject: [PATCH 01/27] ported roboclaw driver from older commits into newest develop branch --- boards/px4/fmu-v5x/default.px4board | 1 + msg/ActuatorControls.msg | 28 ++ msg/CMakeLists.txt | 2 + msg/WheelEncoders.msg | 5 + src/drivers/roboclaw/CMakeLists.txt | 50 ++ src/drivers/roboclaw/Kconfig | 5 + src/drivers/roboclaw/RoboClaw.cpp | 610 +++++++++++++++++++++++++ src/drivers/roboclaw/RoboClaw.hpp | 245 ++++++++++ src/drivers/roboclaw/module.yaml | 6 + src/drivers/roboclaw/roboclaw_main.cpp | 205 +++++++++ src/drivers/roboclaw/roboclaw_params.c | 114 +++++ 11 files changed, 1271 insertions(+) create mode 100644 msg/ActuatorControls.msg create mode 100644 msg/WheelEncoders.msg create mode 100644 src/drivers/roboclaw/CMakeLists.txt create mode 100644 src/drivers/roboclaw/Kconfig create mode 100644 src/drivers/roboclaw/RoboClaw.cpp create mode 100644 src/drivers/roboclaw/RoboClaw.hpp create mode 100644 src/drivers/roboclaw/module.yaml create mode 100644 src/drivers/roboclaw/roboclaw_main.cpp create mode 100644 src/drivers/roboclaw/roboclaw_params.c diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 8873fe41e6d5..c9300dba0703 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -17,6 +17,7 @@ CONFIG_DRIVERS_CAMERA_TRIGGER=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y +CONFIG_DRIVERS_ROBOCLAW=y CONFIG_DRIVERS_GPIO_MCP23009=y CONFIG_DRIVERS_GPS=y CONFIG_DRIVERS_HEATER=y diff --git a/msg/ActuatorControls.msg b/msg/ActuatorControls.msg new file mode 100644 index 000000000000..c4b52ea48dfe --- /dev/null +++ b/msg/ActuatorControls.msg @@ -0,0 +1,28 @@ +uint64 timestamp # time since system start (microseconds) +uint8 NUM_ACTUATOR_CONTROLS = 8 +uint8 NUM_ACTUATOR_CONTROL_GROUPS = 6 +uint8 INDEX_ROLL = 0 +uint8 INDEX_PITCH = 1 +uint8 INDEX_YAW = 2 +uint8 INDEX_THROTTLE = 3 +uint8 INDEX_FLAPS = 4 +uint8 INDEX_SPOILERS = 5 +uint8 INDEX_AIRBRAKES = 6 +uint8 INDEX_LANDING_GEAR = 7 +uint8 INDEX_GIMBAL_SHUTTER = 3 +uint8 INDEX_CAMERA_ZOOM = 4 + +uint8 GROUP_INDEX_ATTITUDE = 0 +uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1 +uint8 GROUP_INDEX_GIMBAL = 2 +uint8 GROUP_INDEX_MANUAL_PASSTHROUGH = 3 +uint8 GROUP_INDEX_ALLOCATED_PART1 = 4 +uint8 GROUP_INDEX_ALLOCATED_PART2 = 5 +uint8 GROUP_INDEX_PAYLOAD = 6 + +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[8] control + +# TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3 +# TOPICS actuator_controls_4 actuator_controls_5 +# TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 826f1cb6ab88..ea3dff55ec30 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -169,6 +169,7 @@ set(msg_files RegisterExtComponentReply.msg RegisterExtComponentRequest.msg Rpm.msg + ActuatorControls.msg RtlTimeEstimate.msg SatelliteInfo.msg SensorAccel.msg @@ -235,6 +236,7 @@ set(msg_files VehicleTrajectoryWaypoint.msg VtolVehicleStatus.msg Wind.msg + WheelEncoders.msg YawEstimatorStatus.msg ) list(SORT msg_files) diff --git a/msg/WheelEncoders.msg b/msg/WheelEncoders.msg new file mode 100644 index 000000000000..1654902cdf00 --- /dev/null +++ b/msg/WheelEncoders.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +int64 encoder_position # The wheel position, in encoder counts since boot. Positive is forward rotation, negative is reverse rotation +int32 speed # Speed of each wheel, in encoder counts per second. Positive is forward, negative is reverse +uint32 pulses_per_rev # Number of pulses per revolution for each wheel diff --git a/src/drivers/roboclaw/CMakeLists.txt b/src/drivers/roboclaw/CMakeLists.txt new file mode 100644 index 000000000000..daad2f40b97c --- /dev/null +++ b/src/drivers/roboclaw/CMakeLists.txt @@ -0,0 +1,50 @@ +############################################################################ +# +# Copyright (c) 2015 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. +# +############################################################################ + +set(PARAM_PREFIX ROBOCLAW) + +if(CONFIG_BOARD_IO) + set(PARAM_PREFIX ROBOCLAW) +endif() + +px4_add_module( + MODULE drivers__roboclaw + MAIN roboclaw + COMPILE_FLAGS + SRCS + roboclaw_main.cpp + RoboClaw.cpp + MODULE_CONFIG + module.yaml + ) + diff --git a/src/drivers/roboclaw/Kconfig b/src/drivers/roboclaw/Kconfig new file mode 100644 index 000000000000..696c14023346 --- /dev/null +++ b/src/drivers/roboclaw/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_ROBOCLAW + bool "crsf_rc" + default n + ---help--- + Enable support for roboclaw diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp new file mode 100644 index 000000000000..89c2ee783e7c --- /dev/null +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -0,0 +1,610 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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.cpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * + */ + +#include "RoboClaw.hpp" +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +// The RoboClaw has a serial communication timeout of 10ms. +// 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, const char *baudRateParam): + _uart(0), + _uart_set(), + _uart_timeout{.tv_sec = 0, .tv_usec = TIMEOUT_US}, + _actuatorsSub(-1), + _lastEncoderCount{0, 0}, + _encoderCounts{0, 0}, + _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(baudRateParam); + _param_handles.address = param_find("RBCLW_ADDRESS"); + + _parameters_update(); + + // start serial port + _uart = open(deviceName, O_RDWR | O_NOCTTY); + + if (_uart < 0) { err(1, "could not open %s", deviceName); } + + int ret = 0; + struct termios uart_config {}; + ret = tcgetattr(_uart, &uart_config); + + if (ret < 0) { err(1, "failed to get attr"); } + + uart_config.c_oflag &= ~ONLCR; // no CR for every LF + ret = cfsetispeed(&uart_config, _parameters.serial_baud_rate); + + if (ret < 0) { err(1, "failed to set input speed"); } + + ret = cfsetospeed(&uart_config, _parameters.serial_baud_rate); + + if (ret < 0) { err(1, "failed to set output speed"); } + + ret = tcsetattr(_uart, TCSANOW, &uart_config); + + if (ret < 0) { err(1, "failed to set attr"); } + + FD_ZERO(&_uart_set); + + // setup default settings, reset encoders + resetEncoders(); +} + +RoboClaw::~RoboClaw() +{ + setMotorDutyCycle(MOTOR_1, 0.0); + setMotorDutyCycle(MOTOR_2, 0.0); + close(_uart); +} + +void RoboClaw::taskMain() +{ + // Make sure the Roboclaw is actually connected, so I don't just spam errors if it's not. + uint8_t rbuff[4]; + int err_code = _transaction(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true); + + if (err_code <= 0) { + PX4_ERR("Unable to connect to Roboclaw. Shutting down Roboclaw driver."); + return; + } + + // This main loop performs two different tasks, asynchronously: + // - Send actuator_controls_0 to the Roboclaw as soon as they are available + // - Read the encoder values at a constant rate + // To do this, the timeout on the poll() function is used. + // waitTime is the amount of time left (int microseconds) until the next time I should read from the encoders. + // It is updated at the end of every loop. Sometimes, if the actuator_controls_0 message came in right before + // I should have read the encoders, waitTime will be 0. This is fine. When waitTime is 0, poll() will return + // immediately with a timeout. (Or possibly with a message, if one happened to be available at that exact moment) + uint64_t encoderTaskLastRun = 0; + int waitTime = 0; + + uint64_t actuatorsLastWritten = 0; + + _actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0)); + orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms); + + _armedSub = orb_subscribe(ORB_ID(actuator_armed)); + _paramSub = orb_subscribe(ORB_ID(parameter_update)); + + pollfd fds[3]; + fds[0].fd = _paramSub; + fds[0].events = POLLIN; + fds[1].fd = _actuatorsSub; + fds[1].events = POLLIN; + fds[2].fd = _armedSub; + fds[2].events = POLLIN; + + memset((void *) &_wheelEncoderMsg[0], 0, sizeof(_wheelEncoderMsg)); + _wheelEncoderMsg[0].pulses_per_rev = _parameters.counts_per_rev; + _wheelEncoderMsg[1].pulses_per_rev = _parameters.counts_per_rev; + + while (!taskShouldExit) { + + int pret = poll(fds, sizeof(fds) / sizeof(pollfd), waitTime / 1000); + + bool actuators_timeout = int(hrt_absolute_time() - actuatorsLastWritten) > 2000 * _parameters.actuator_write_period_ms; + + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(parameter_update), _paramSub, &_paramUpdate); + _parameters_update(); + } + + // No timeout, update on either the actuator controls or the armed state + if (pret > 0 && (fds[1].revents & POLLIN || fds[2].revents & POLLIN || actuators_timeout)) { + orb_copy(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuatorControls); + orb_copy(ORB_ID(actuator_armed), _armedSub, &_actuatorArmed); + + int drive_ret = 0, turn_ret = 0; + + const bool disarmed = !_actuatorArmed.armed || _actuatorArmed.lockdown || _actuatorArmed.manual_lockdown + || _actuatorArmed.force_failsafe || actuators_timeout; + + if (disarmed) { + // If disarmed, I want to be certain that the stop command gets through. + int tries = 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); + } + + } else { + drive_ret = drive(_actuatorControls.control[actuator_controls_s::INDEX_THROTTLE]); + turn_ret = turn(_actuatorControls.control[actuator_controls_s::INDEX_YAW]); + + if (drive_ret <= 0 || turn_ret <= 0) { + PX4_ERR("Error controlling RoboClaw. Drive err: %d. Turn err: %d", drive_ret, turn_ret); + } + } + + actuatorsLastWritten = hrt_absolute_time(); + + } else { + // A timeout occurred, which means that it's time to update the encoders + encoderTaskLastRun = hrt_absolute_time(); + + if (readEncoder() > 0) { + + for (int i = 0; i < 2; i++) { + _wheelEncoderMsg[i].timestamp = encoderTaskLastRun; + + _wheelEncoderMsg[i].encoder_position = _encoderCounts[i]; + _wheelEncoderMsg[i].speed = _motorSpeeds[i]; + + _wheelEncodersAdv[i].publish(_wheelEncoderMsg[i]); + } + + } else { + PX4_ERR("Error reading encoders"); + } + } + + waitTime = _parameters.encoder_read_period_ms * 1000 - (hrt_absolute_time() - encoderTaskLastRun); + waitTime = waitTime < 0 ? 0 : waitTime; + } + + orb_unsubscribe(_actuatorsSub); + orb_unsubscribe(_armedSub); + orb_unsubscribe(_paramSub); +} + +int RoboClaw::readEncoder() +{ + + uint8_t rbuff_pos[ENCODER_MESSAGE_SIZE]; + // 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 < TIMEOUT_RETRIES && !success; retry++) { + success = _transaction(CMD_READ_BOTH_ENCODERS, nullptr, 0, &rbuff_pos[0], ENCODER_MESSAGE_SIZE, false, + true) == ENCODER_MESSAGE_SIZE; + success = success && _transaction(CMD_READ_SPEED_1, nullptr, 0, &rbuff_speed[0], ENCODER_SPEED_MESSAGE_SIZE, false, + true) == ENCODER_SPEED_MESSAGE_SIZE; + success = success && _transaction(CMD_READ_SPEED_2, nullptr, 0, &rbuff_speed[4], ENCODER_SPEED_MESSAGE_SIZE, false, + true) == ENCODER_SPEED_MESSAGE_SIZE; + } + + if (!success) { + PX4_ERR("Error reading encoders"); + return -1; + } + + uint32_t count; + uint32_t speed; + uint8_t *count_bytes; + uint8_t *speed_bytes; + + for (int motor = 0; motor <= 1; motor++) { + count = 0; + speed = 0; + count_bytes = &rbuff_pos[motor * 4]; + speed_bytes = &rbuff_speed[motor * 4]; + + // Data from the roboclaw is big-endian. This converts the bytes to an integer, regardless of the + // endianness of the system this code is running on. + for (int byte = 0; byte < 4; byte++) { + count = (count << 8) + count_bytes[byte]; + speed = (speed << 8) + speed_bytes[byte]; + } + + // The Roboclaw stores encoder counts as unsigned 32-bit ints. This can overflow, especially when starting + // at 0 and moving backward. The Roboclaw has overflow flags for this, but in my testing, they don't seem + // to work. This code detects overflow manually. + // These diffs are the difference between the count I just read from the Roboclaw and the last + // count that was read from the roboclaw for this motor. fwd_diff assumes that the wheel moved forward, + // and rev_diff assumes it moved backward. If the motor actually moved forward, then rev_diff will be close + // to 2^32 (UINT32_MAX). If the motor actually moved backward, then fwd_diff will be close to 2^32. + // To detect and account for overflow, I just assume that the smaller diff is correct. + // Strictly speaking, if the wheel rotated more than 2^31 encoder counts since the last time I checked, this + // will be wrong. But that's 1.7 million revolutions, so it probably won't come up. + uint32_t fwd_diff = count - _lastEncoderCount[motor]; + uint32_t rev_diff = _lastEncoderCount[motor] - count; + // At this point, abs(diff) is always <= 2^31, so this cast from unsigned to signed is safe. + int32_t diff = fwd_diff <= rev_diff ? fwd_diff : -int32_t(rev_diff); + _encoderCounts[motor] += diff; + _lastEncoderCount[motor] = count; + + _motorSpeeds[motor] = speed; + } + + return 1; +} + +void RoboClaw::printStatus(char *string, size_t n) +{ + snprintf(string, n, "pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n", + double(getMotorPosition(MOTOR_1)), + double(getMotorSpeed(MOTOR_1)), + double(getMotorPosition(MOTOR_2)), + double(getMotorSpeed(MOTOR_2))); +} + +float RoboClaw::getMotorPosition(e_motor motor) +{ + if (motor == MOTOR_1) { + return _encoderCounts[0]; + + } else if (motor == MOTOR_2) { + return _encoderCounts[1]; + + } else { + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; + } +} + +float RoboClaw::getMotorSpeed(e_motor motor) +{ + if (motor == MOTOR_1) { + return _motorSpeeds[0]; + + } else if (motor == MOTOR_2) { + return _motorSpeeds[1]; + + } else { + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; + } +} + +int RoboClaw::setMotorSpeed(e_motor motor, float value) +{ + e_command command; + + // send command + if (motor == MOTOR_1) { + if (value > 0) { + command = CMD_DRIVE_FWD_1; + + } else { + command = CMD_DRIVE_REV_1; + } + + } else if (motor == MOTOR_2) { + if (value > 0) { + command = CMD_DRIVE_FWD_2; + + } else { + command = CMD_DRIVE_REV_2; + } + + } else { + return -1; + } + + return _sendUnsigned7Bit(command, value); +} + +int RoboClaw::setMotorDutyCycle(e_motor motor, float value) +{ + + e_command command; + + // send command + if (motor == MOTOR_1) { + command = CMD_SIGNED_DUTYCYCLE_1; + + } else if (motor == MOTOR_2) { + command = CMD_SIGNED_DUTYCYCLE_2; + + } else { + return -1; + } + + return _sendSigned16Bit(command, value); +} + +int RoboClaw::drive(float value) +{ + e_command command = value >= 0 ? CMD_DRIVE_FWD_MIX : CMD_DRIVE_REV_MIX; + return _sendUnsigned7Bit(command, value); +} + +int RoboClaw::turn(float value) +{ + e_command command = value >= 0 ? CMD_TURN_LEFT : CMD_TURN_RIGHT; + return _sendUnsigned7Bit(command, value); +} + +int RoboClaw::resetEncoders() +{ + return _sendNothing(CMD_RESET_ENCODERS); +} + +int RoboClaw::_sendUnsigned7Bit(e_command command, float data) +{ + data = fabs(data); + + if (data > 1.0f) { + data = 1.0f; + } + + auto byte = (uint8_t)(data * INT8_MAX); + uint8_t recv_byte; + return _transaction(command, &byte, 1, &recv_byte, 1); +} + +int RoboClaw::_sendSigned16Bit(e_command command, float data) +{ + if (data > 1.0f) { + data = 1.0f; + + } else if (data < -1.0f) { + data = -1.0f; + } + + auto buff = (uint16_t)(data * INT16_MAX); + uint8_t recv_buff; + return _transaction(command, (uint8_t *) &buff, 2, &recv_buff, 1); +} + +int RoboClaw::_sendNothing(e_command command) +{ + uint8_t recv_buff; + return _transaction(command, nullptr, 0, &recv_buff, 1); +} + +uint16_t RoboClaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init) +{ + uint16_t crc = init; + + for (size_t byte = 0; byte < n; byte++) { + crc = crc ^ (((uint16_t) buf[byte]) << 8); + + for (uint8_t bit = 0; bit < 8; bit++) { + if (crc & 0x8000) { + crc = (crc << 1) ^ 0x1021; + + } else { + crc = crc << 1; + } + } + } + + return crc; +} + +int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, + uint8_t *rbuff, size_t rbytes, bool send_checksum, bool recv_checksum) +{ + int err_code = 0; + + // WRITE + + tcflush(_uart, TCIOFLUSH); // flush buffers + uint8_t buf[wbytes + 4]; + buf[0] = (uint8_t) _parameters.address; + buf[1] = cmd; + + if (wbuff) { + memcpy(&buf[2], wbuff, wbytes); + } + + wbytes = wbytes + (send_checksum ? 4 : 2); + + if (send_checksum) { + uint16_t sum = _calcCRC(buf, wbytes - 2); + buf[wbytes - 2] = (sum >> 8) & 0xFF; + buf[wbytes - 1] = sum & 0xFFu; + } + + int count = write(_uart, buf, wbytes); + + if (count < (int) wbytes) { // Did not successfully send all bytes. + PX4_ERR("Only wrote %d out of %zu bytes", count, wbytes); + return -1; + } + + // READ + + FD_ZERO(&_uart_set); + FD_SET(_uart, &_uart_set); + + uint8_t *rbuff_curr = rbuff; + size_t bytes_read = 0; + + // select(...) returns as soon as even 1 byte is available. read(...) returns immediately, no matter how many + // bytes are available. I need to keep reading until I get the number of bytes I expect. + while (bytes_read < rbytes) { + // select(...) may change this timeout struct (because it is not const). So I reset it every time. + _uart_timeout.tv_sec = 0; + _uart_timeout.tv_usec = TIMEOUT_US; + err_code = select(_uart + 1, &_uart_set, nullptr, nullptr, &_uart_timeout); + + // An error code of 0 means that select timed out, which is how the Roboclaw indicates an error. + if (err_code <= 0) { + return err_code; + } + + err_code = read(_uart, rbuff_curr, rbytes - bytes_read); + + if (err_code <= 0) { + return err_code; + + } else { + bytes_read += err_code; + rbuff_curr += err_code; + } + } + + //TODO: Clean up this mess of IFs and returns + + if (recv_checksum) { + if (bytes_read < 2) { + return -1; + } + + // The checksum sent back by the roboclaw is calculated based on the address and command bytes as well + // as the data returned. + uint16_t checksum_calc = _calcCRC(buf, 2); + checksum_calc = _calcCRC(rbuff, bytes_read - 2, checksum_calc); + uint16_t checksum_recv = (rbuff[bytes_read - 2] << 8) + rbuff[bytes_read - 1]; + + if (checksum_calc == checksum_recv) { + return bytes_read; + + } else { + return -10; + } + + } else { + if (bytes_read == 1 && rbuff[0] == 0xFF) { + return 1; + + } else { + return -11; + } + } +} + +void RoboClaw::_parameters_update() +{ + 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); + + if (_actuatorsSub > 0) { + orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms); + } + + int32_t baudRate; + param_get(_param_handles.serial_baud_rate, &baudRate); + + switch (baudRate) { + case 2400: + _parameters.serial_baud_rate = B2400; + break; + + case 9600: + _parameters.serial_baud_rate = B9600; + break; + + case 19200: + _parameters.serial_baud_rate = B19200; + break; + + case 38400: + _parameters.serial_baud_rate = B38400; + break; + + case 57600: + _parameters.serial_baud_rate = B57600; + break; + + case 115200: + _parameters.serial_baud_rate = B115200; + break; + + case 230400: + _parameters.serial_baud_rate = B230400; + break; + + case 460800: + _parameters.serial_baud_rate = B460800; + break; + + default: + _parameters.serial_baud_rate = B2400; + } +} diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp new file mode 100644 index 000000000000..0a92d07f3993 --- /dev/null +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -0,0 +1,245 @@ +/**************************************************************************** + * + * Copyright (C) 2013 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 RoboClas.hpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * This is a driver for the RoboClaw motor controller + */ +class RoboClaw +{ +public: + + void taskMain(); + static bool taskShouldExit; + + /** control channels */ + enum e_channel { + CH_VOLTAGE_LEFT = 0, + CH_VOLTAGE_RIGHT + }; + + /** motors */ + enum e_motor { + MOTOR_1 = 0, + MOTOR_2 + }; + + /** + * constructor + * @param deviceName the name of the + * serial port e.g. "/dev/ttyS2" + * @param address the adddress of the motor + * (selectable on roboclaw) + * @param baudRateParam Name of the parameter that holds the baud rate of this serial port + */ + RoboClaw(const char *deviceName, const char *baudRateParam); + + /** + * deconstructor + */ + virtual ~RoboClaw(); + + /** + * @return position of a motor, rev + */ + float getMotorPosition(e_motor motor); + + /** + * @return speed of a motor, rev/sec + */ + float getMotorSpeed(e_motor motor); + + /** + * set the speed of a motor, rev/sec + */ + int setMotorSpeed(e_motor motor, float value); + + /** + * set the duty cycle of a motor + */ + int setMotorDutyCycle(e_motor motor, float value); + + /** + * Drive both motors. +1 = full forward, -1 = full backward + */ + int drive(float value); + + /** + * Turn. +1 = full right, -1 = full left + */ + int turn(float value); + + /** + * reset the encoders + * @return status + */ + int resetEncoders(); + + /** + * read data from serial + */ + int readEncoder(); + + /** + * print status + */ + void printStatus(char *string, size_t n); + +private: + + // commands + // We just list the commands we want from the manual here. + enum e_command { + + // simple + CMD_DRIVE_FWD_1 = 0, + CMD_DRIVE_REV_1 = 1, + CMD_DRIVE_FWD_2 = 4, + CMD_DRIVE_REV_2 = 5, + + CMD_DRIVE_FWD_MIX = 8, + CMD_DRIVE_REV_MIX = 9, + CMD_TURN_RIGHT = 10, + CMD_TURN_LEFT = 11, + + // encoder commands + CMD_READ_ENCODER_1 = 16, + CMD_READ_ENCODER_2 = 17, + CMD_READ_SPEED_1 = 18, + CMD_READ_SPEED_2 = 19, + CMD_RESET_ENCODERS = 20, + CMD_READ_BOTH_ENCODERS = 78, + CMD_READ_BOTH_SPEEDS = 79, + + // advanced motor control + CMD_READ_SPEED_HIRES_1 = 30, + CMD_READ_SPEED_HIRES_2 = 31, + CMD_SIGNED_DUTYCYCLE_1 = 32, + CMD_SIGNED_DUTYCYCLE_2 = 33, + + CMD_READ_STATUS = 90 + }; + + struct { + speed_t serial_baud_rate; + int32_t counts_per_rev; + 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 encoder_read_period_ms; + param_t actuator_write_period_ms; + param_t address; + } _param_handles{}; + + int _uart; + fd_set _uart_set; + struct timeval _uart_timeout; + + /** actuator controls subscription */ + int _actuatorsSub{-1}; + actuator_controls_s _actuatorControls; + + int _armedSub{-1}; + actuator_armed_s _actuatorArmed; + + int _paramSub{-1}; + parameter_update_s _paramUpdate; + + uORB::PublicationMulti _wheelEncodersAdv[2] { ORB_ID(wheel_encoders), ORB_ID(wheel_encoders)}; + wheel_encoders_s _wheelEncoderMsg[2]; + + uint32_t _lastEncoderCount[2] {0, 0}; + int64_t _encoderCounts[2] {0, 0}; + int32_t _motorSpeeds[2] {0, 0}; + + 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); + int _sendNothing(e_command); + + /** + * Perform a round-trip write and read. + * + * NOTE: This function is not thread-safe. + * + * @param cmd Command to send to the Roboclaw + * @param wbuff Write buffer. Must not contain command, address, or checksum. For most commands, this will be + * one or two bytes. Can be null iff wbytes == 0. + * @param wbytes Number of bytes to write. Can be 0. + * @param rbuff Read buffer. Will be filled with the entire response, including a checksum if the Roboclaw sends + * a checksum for this command. + * @param rbytes Maximum number of bytes to read. + * @param send_checksum If true, then the checksum will be calculated and sent to the Roboclaw. + * This is an option because some Roboclaw commands expect no checksum. + * @param recv_checksum If true, then this function will calculate the checksum of the returned data and compare + * it to the checksum received. If they are not equal, OR if fewer than 2 bytes were received, then an + * error is returned. + * If false, then this function will expect to read exactly one byte, 0xFF, and will return an error otherwise. + * @return If successful, then the number of bytes read from the Roboclaw is returned. If there is a timeout + * reading from the Roboclaw, then 0 is returned. If there is an IO error, then a negative value is returned. + */ + int _transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, + uint8_t *rbuff, size_t rbytes, bool send_checksum = true, bool recv_checksum = false); +}; diff --git a/src/drivers/roboclaw/module.yaml b/src/drivers/roboclaw/module.yaml new file mode 100644 index 000000000000..81e5f694dfee --- /dev/null +++ b/src/drivers/roboclaw/module.yaml @@ -0,0 +1,6 @@ +module_name: Roboclaw Driver +serial_config: + - command: roboclaw start ${SERIAL_DEV} ${BAUD_PARAM} + port_config_param: + name: RBCLW_SER_CFG + group: Roboclaw \ No newline at end of file diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp new file mode 100644 index 000000000000..44c9ca8d6bc3 --- /dev/null +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -0,0 +1,205 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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_main.cpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.ionmc.com/docs/roboclaw_user_manual.pdf + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + + +#include "RoboClaw.hpp" + +static bool thread_running = false; /**< Deamon status flag */ +px4_task_t deamon_task; + +/** + * Deamon management function. + */ +extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int roboclaw_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(); + +static void usage() +{ + PRINT_MODULE_USAGE_NAME("roboclaw", "driver"); + + PRINT_MODULE_DESCRIPTION(R"DESCR_STR( +### Description + +This driver communicates over UART with the [Roboclaw motor driver](http://downloads.basicmicro.com/docs/roboclaw_user_manual.pdf). +It performs two tasks: + + - Control the motors based on the `actuator_controls_0` UOrb topic. + - Read the wheel encoders and publish the raw data in the `wheel_encoders` UOrb topic + +In order to use this driver, the Roboclaw should be put into Packet Serial mode (see the linked documentation), and +your flight controller's UART port should be connected to the Roboclaw as shown in the documentation. For Pixhawk 4, +use the `UART & I2C B` port, which corresponds to `/dev/ttyS3`. + +### Implementation + +The main loop of this module (Located in `RoboClaw.cpp::task_main()`) performs 2 tasks: + + 1. Write `actuator_controls_0` messages to the Roboclaw as they become available + 2. Read encoder data from the Roboclaw at a constant, fixed rate. + +Because of the latency of UART, this driver does not write every single `actuator_controls_0` message to the Roboclaw +immediately. Instead, it is rate limited based on the parameter `RBCLW_WRITE_PER`. + +On startup, this driver will attempt to read the status of the Roboclaw to verify that it is connected. If this fails, +the driver terminates immediately. + +### Examples + +The command to start this driver is: + + $ roboclaw start + +`` is the name of the UART port. On the Pixhawk 4, this is `/dev/ttyS3`. +`` is te baud rate. + +All available commands are: + + - `$ roboclaw start ` + - `$ roboclaw status` + - `$ roboclaw stop` + )DESCR_STR"); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int roboclaw_main(int argc, char *argv[]) +{ + + if (argc < 4) { + usage(); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("roboclaw already running\n"); + /* this is not an error */ + return 0; + } + + RoboClaw::taskShouldExit = false; + deamon_task = px4_task_spawn_cmd("roboclaw", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 2000, + roboclaw_thread_main, + (char *const *)argv); + return 0; + + } else if (!strcmp(argv[1], "stop")) { + + RoboClaw::taskShouldExit = true; + return 0; + + } else if (!strcmp(argv[1], "status")) { + + if (thread_running) { + printf("\troboclaw app is running\n"); + + } else { + printf("\troboclaw app not started\n"); + } + + return 0; + } + + usage(); + return 1; +} + +int roboclaw_thread_main(int argc, char *argv[]) +{ + printf("[roboclaw] starting\n"); + + // skip parent process args + argc -= 2; + argv += 2; + + if (argc < 2) { + printf("usage: roboclaw start \n"); + return -1; + } + + const char *deviceName = argv[1]; + const char *baudRate = argv[2]; + + // start + RoboClaw roboclaw(deviceName, baudRate); + + thread_running = true; + + roboclaw.taskMain(); + + // exit + printf("[roboclaw] exiting.\n"); + thread_running = false; + return 0; +} diff --git a/src/drivers/roboclaw/roboclaw_params.c b/src/drivers/roboclaw/roboclaw_params.c new file mode 100644 index 000000000000..3f3c938c55c0 --- /dev/null +++ b/src/drivers/roboclaw/roboclaw_params.c @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * 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); + +/** + * 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); + +/** + * Roboclaw serial baud rate + * + * Baud rate of the serial communication with the Roboclaw. The Roboclaw must be configured to match this rate. + * @min 2400 + * @max 460800 + * @value 2400 2400 baud + * @value 9600 9600 baud + * @value 19200 19200 baud + * @value 38400 38400 baud + * @value 57600 57600 baud + * @value 115200 115200 baud + * @value 230400 230400 baud + * @value 460800 460800 baud + * @group Roboclaw driver + * @reboot_required true + */ +PARAM_DEFINE_INT32(RBCLW_BAUD, 2400); From 3bf912255ddee4c32ef61e2a614c23ce8aac8614 Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Mon, 9 Oct 2023 18:11:01 +0200 Subject: [PATCH 02/27] Roboclaw: Temporary fix, enabling driver to run --- .../px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover | 1 - src/drivers/roboclaw/RoboClaw.cpp | 2 +- src/drivers/roboclaw/roboclaw_params.c | 2 +- 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover index 53c857a16e28..c8940fcc77c7 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover @@ -58,4 +58,3 @@ param set-default PWM_MAIN_DIS1 1500 param set-default PWM_MAIN_DIS2 1500 param set-default PWM_MAIN_TIM0 50 param set-default PWM_MAIN_TIM1 50 - diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 89c2ee783e7c..d0ce86c2368e 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -133,7 +133,7 @@ RoboClaw::~RoboClaw() void RoboClaw::taskMain() { // Make sure the Roboclaw is actually connected, so I don't just spam errors if it's not. - uint8_t rbuff[4]; + uint8_t rbuff[6]; int err_code = _transaction(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true); if (err_code <= 0) { diff --git a/src/drivers/roboclaw/roboclaw_params.c b/src/drivers/roboclaw/roboclaw_params.c index 3f3c938c55c0..849d95663812 100644 --- a/src/drivers/roboclaw/roboclaw_params.c +++ b/src/drivers/roboclaw/roboclaw_params.c @@ -111,4 +111,4 @@ PARAM_DEFINE_INT32(RBCLW_ADDRESS, 128); * @group Roboclaw driver * @reboot_required true */ -PARAM_DEFINE_INT32(RBCLW_BAUD, 2400); +PARAM_DEFINE_INT32(RBCLW_BAUD, 57600); From 29c12706e0fc797c63dbbc4e39191ccc8803a764 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 14 Dec 2021 16:48:22 +0100 Subject: [PATCH 03/27] Driving possible --- src/drivers/roboclaw/RoboClaw.cpp | 116 ++++++++---------- src/drivers/roboclaw/RoboclawSerialDevice.hpp | 66 ++++++++++ src/drivers/roboclaw/module.yaml | 2 +- src/drivers/roboclaw/roboclaw_main.cpp | 13 +- 4 files changed, 121 insertions(+), 76 deletions(-) create mode 100644 src/drivers/roboclaw/RoboclawSerialDevice.hpp diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index d0ce86c2368e..82276e439665 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -55,6 +55,7 @@ #include #include #include +#include // The RoboClaw has a serial communication timeout of 10ms. // Add a little extra to account for timing inaccuracy @@ -75,6 +76,8 @@ bool RoboClaw::taskShouldExit = false; +using namespace time_literals; + RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam): _uart(0), _uart_set(), @@ -88,6 +91,7 @@ RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam): _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"); + printf("Baudrate parameter: %s\n", baudRateParam); _param_handles.serial_baud_rate = param_find(baudRateParam); _param_handles.address = param_find("RBCLW_ADDRESS"); @@ -105,6 +109,7 @@ RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam): if (ret < 0) { err(1, "failed to get attr"); } uart_config.c_oflag &= ~ONLCR; // no CR for every LF + uart_config.c_cflag &= ~CRTSCTS; ret = cfsetispeed(&uart_config, _parameters.serial_baud_rate); if (ret < 0) { err(1, "failed to set input speed"); } @@ -118,15 +123,16 @@ RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam): if (ret < 0) { err(1, "failed to set attr"); } FD_ZERO(&_uart_set); + FD_SET(_uart, &_uart_set); // setup default settings, reset encoders - resetEncoders(); + //resetEncoders(); } RoboClaw::~RoboClaw() { - setMotorDutyCycle(MOTOR_1, 0.0); - setMotorDutyCycle(MOTOR_2, 0.0); + // setMotorDutyCycle(MOTOR_1, 0.0); + // setMotorDutyCycle(MOTOR_2, 0.0); close(_uart); } @@ -149,10 +155,7 @@ void RoboClaw::taskMain() // It is updated at the end of every loop. Sometimes, if the actuator_controls_0 message came in right before // I should have read the encoders, waitTime will be 0. This is fine. When waitTime is 0, poll() will return // immediately with a timeout. (Or possibly with a message, if one happened to be available at that exact moment) - uint64_t encoderTaskLastRun = 0; - int waitTime = 0; - - uint64_t actuatorsLastWritten = 0; + int waitTime = 100_ms; _actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0)); orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms); @@ -168,74 +171,35 @@ void RoboClaw::taskMain() fds[2].fd = _armedSub; fds[2].events = POLLIN; - memset((void *) &_wheelEncoderMsg[0], 0, sizeof(_wheelEncoderMsg)); - _wheelEncoderMsg[0].pulses_per_rev = _parameters.counts_per_rev; - _wheelEncoderMsg[1].pulses_per_rev = _parameters.counts_per_rev; - while (!taskShouldExit) { int pret = poll(fds, sizeof(fds) / sizeof(pollfd), waitTime / 1000); - bool actuators_timeout = int(hrt_absolute_time() - actuatorsLastWritten) > 2000 * _parameters.actuator_write_period_ms; - if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(parameter_update), _paramSub, &_paramUpdate); _parameters_update(); } // No timeout, update on either the actuator controls or the armed state - if (pret > 0 && (fds[1].revents & POLLIN || fds[2].revents & POLLIN || actuators_timeout)) { + if (pret > 0 && (fds[1].revents & POLLIN || fds[2].revents & POLLIN)) { orb_copy(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuatorControls); orb_copy(ORB_ID(actuator_armed), _armedSub, &_actuatorArmed); - int drive_ret = 0, turn_ret = 0; - const bool disarmed = !_actuatorArmed.armed || _actuatorArmed.lockdown || _actuatorArmed.manual_lockdown - || _actuatorArmed.force_failsafe || actuators_timeout; + || _actuatorArmed.force_failsafe; if (disarmed) { - // If disarmed, I want to be certain that the stop command gets through. - int tries = 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); - } + setMotorDutyCycle(MOTOR_1, 0.f); + setMotorDutyCycle(MOTOR_2, 0.f); } else { - drive_ret = drive(_actuatorControls.control[actuator_controls_s::INDEX_THROTTLE]); - turn_ret = turn(_actuatorControls.control[actuator_controls_s::INDEX_YAW]); - - if (drive_ret <= 0 || turn_ret <= 0) { - PX4_ERR("Error controlling RoboClaw. Drive err: %d. Turn err: %d", drive_ret, turn_ret); - } - } - - actuatorsLastWritten = hrt_absolute_time(); - - } else { - // A timeout occurred, which means that it's time to update the encoders - encoderTaskLastRun = hrt_absolute_time(); - - if (readEncoder() > 0) { - - for (int i = 0; i < 2; i++) { - _wheelEncoderMsg[i].timestamp = encoderTaskLastRun; - - _wheelEncoderMsg[i].encoder_position = _encoderCounts[i]; - _wheelEncoderMsg[i].speed = _motorSpeeds[i]; - - _wheelEncodersAdv[i].publish(_wheelEncoderMsg[i]); - } - - } else { - PX4_ERR("Error reading encoders"); + const float throttle = _actuatorControls.control[actuator_controls_s::INDEX_THROTTLE]; + const float yaw = _actuatorControls.control[actuator_controls_s::INDEX_YAW]; + const float scale = 0.3f; + setMotorDutyCycle(MOTOR_1, (throttle - yaw) * scale); + setMotorDutyCycle(MOTOR_2, (throttle + yaw) * scale); } } - - waitTime = _parameters.encoder_read_period_ms * 1000 - (hrt_absolute_time() - encoderTaskLastRun); - waitTime = waitTime < 0 ? 0 : waitTime; } orb_unsubscribe(_actuatorsSub); @@ -426,16 +390,12 @@ int RoboClaw::_sendUnsigned7Bit(e_command command, float data) int RoboClaw::_sendSigned16Bit(e_command command, float data) { - if (data > 1.0f) { - data = 1.0f; - - } else if (data < -1.0f) { - data = -1.0f; - } - - auto buff = (uint16_t)(data * INT16_MAX); + int16_t duty = math::constrain(data, -1.f, 1.f) * INT16_MAX; + uint8_t buff[2]; + buff[0] = (duty >> 8) & 0xFF; // High byte + buff[1] = duty & 0xFF; // Low byte uint8_t recv_buff; - return _transaction(command, (uint8_t *) &buff, 2, &recv_buff, 1); + return _transaction(command, (uint8_t *) &buff, 2, &recv_buff, 2); } int RoboClaw::_sendNothing(e_command command) @@ -488,6 +448,14 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, buf[wbytes - 1] = sum & 0xFFu; } + // printf("Write: "); + + // for (size_t i = 0; i < wbytes; i++) { + // printf("%X ", buf[i]); + // } + + // printf("\n"); + int count = write(_uart, buf, wbytes); if (count < (int) wbytes) { // Did not successfully send all bytes. @@ -495,30 +463,37 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, return -1; } + // else { + // printf("Successfully wrote %d of %zu bytes\n", count, wbytes); + // } + // READ - FD_ZERO(&_uart_set); - FD_SET(_uart, &_uart_set); uint8_t *rbuff_curr = rbuff; size_t bytes_read = 0; // select(...) returns as soon as even 1 byte is available. read(...) returns immediately, no matter how many // bytes are available. I need to keep reading until I get the number of bytes I expect. + //printf("Read: \n"); while (bytes_read < rbytes) { // select(...) may change this timeout struct (because it is not const). So I reset it every time. _uart_timeout.tv_sec = 0; _uart_timeout.tv_usec = TIMEOUT_US; err_code = select(_uart + 1, &_uart_set, nullptr, nullptr, &_uart_timeout); + //printf("Select: %d\n", err_code); // An error code of 0 means that select timed out, which is how the Roboclaw indicates an error. if (err_code <= 0) { + printf("select error: %d\n", err_code); return err_code; } err_code = read(_uart, rbuff_curr, rbytes - bytes_read); + //printf("Read: %d\n", err_code); if (err_code <= 0) { + printf("read error: %d\n", err_code); return err_code; } else { @@ -527,6 +502,14 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, } } + // printf("Read: "); + + // for (size_t i = 0; i < bytes_read; i++) { + // printf("%X ", rbuff[i]); + // } + + // printf("\n"); + //TODO: Clean up this mess of IFs and returns if (recv_checksum) { @@ -540,6 +523,7 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, checksum_calc = _calcCRC(rbuff, bytes_read - 2, checksum_calc); uint16_t checksum_recv = (rbuff[bytes_read - 2] << 8) + rbuff[bytes_read - 1]; + //printf("crc calc: %X crc rec: %X\n", checksum_calc, checksum_recv); if (checksum_calc == checksum_recv) { return bytes_read; diff --git a/src/drivers/roboclaw/RoboclawSerialDevice.hpp b/src/drivers/roboclaw/RoboclawSerialDevice.hpp new file mode 100644 index 000000000000..f70f3c5385b3 --- /dev/null +++ b/src/drivers/roboclaw/RoboclawSerialDevice.hpp @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (c) 2021 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 RoboclawSerialDevice.hpp + * @brief + * @author Matthias Grob + */ + +#include "RoboclawDriver/RoboclawDriver.hpp" +#include + +class RoboclawDevice : public RoboclawWritableInterface +{ +public: + RoboclawDevice(const char *port); + ~RoboclawDevice(); + int init(); + void printStatus(); + +private: + void Run(); + size_t writeCallback(const uint8_t *buffer, const uint16_t length) override; + int setBaudrate(const unsigned baudrate); + + static constexpr size_t READ_BUFFER_SIZE{256}; + static constexpr int DISARM_VALUE = 0; + static constexpr int MIN_THROTTLE = 100; + static constexpr int MAX_THROTTLE = 1000; + + char _port[20] {}; + int _serial_fd{-1}; + VescDriver _vesc_driver; + MixingOutput _mixing_output{4, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; +}; diff --git a/src/drivers/roboclaw/module.yaml b/src/drivers/roboclaw/module.yaml index 81e5f694dfee..b501b1ecb380 100644 --- a/src/drivers/roboclaw/module.yaml +++ b/src/drivers/roboclaw/module.yaml @@ -3,4 +3,4 @@ serial_config: - command: roboclaw start ${SERIAL_DEV} ${BAUD_PARAM} port_config_param: name: RBCLW_SER_CFG - group: Roboclaw \ No newline at end of file + group: Roboclaw diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp index 44c9ca8d6bc3..a3483c25a548 100644 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -82,11 +82,11 @@ static void usage() PRINT_MODULE_DESCRIPTION(R"DESCR_STR( ### Description -This driver communicates over UART with the [Roboclaw motor driver](http://downloads.basicmicro.com/docs/roboclaw_user_manual.pdf). +This driver communicates over UART with the [Roboclaw motor driver](https://www.basicmicro.com/motor-controller). It performs two tasks: - - Control the motors based on the `actuator_controls_0` UOrb topic. - - Read the wheel encoders and publish the raw data in the `wheel_encoders` UOrb topic + - Control the motors based on the `actuator_controls_0` uORB topic. + - Read the wheel encoders and publish the raw data in the `wheel_encoders` uORB topic In order to use this driver, the Roboclaw should be put into Packet Serial mode (see the linked documentation), and your flight controller's UART port should be connected to the Roboclaw as shown in the documentation. For Pixhawk 4, @@ -132,12 +132,7 @@ All available commands are: */ int roboclaw_main(int argc, char *argv[]) { - - if (argc < 4) { - usage(); - } - - if (!strcmp(argv[1], "start")) { + if (!strcmp(argv[1], "start") && (argc >= 4)) { if (thread_running) { printf("roboclaw already running\n"); From 3aefa72c4d48ee85f8edc41d9633872b20204136 Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Tue, 10 Oct 2023 16:07:34 +0200 Subject: [PATCH 04/27] Roboclaw: Added DutyCycle command in Roboclaw destructor to turn off motors --- src/drivers/roboclaw/RoboClaw.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 82276e439665..669e7bcb2585 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -131,8 +131,8 @@ RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam): RoboClaw::~RoboClaw() { - // setMotorDutyCycle(MOTOR_1, 0.0); - // setMotorDutyCycle(MOTOR_2, 0.0); + setMotorDutyCycle(MOTOR_1, 0.0); + setMotorDutyCycle(MOTOR_2, 0.0); close(_uart); } From f09878f65c704c2362956a7eb5e5a2b5c54481bc Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Tue, 10 Oct 2023 16:04:20 +0200 Subject: [PATCH 05/27] Roboclaw: Working temporary version that drives around --- boards/px4/fmu-v5x/default.px4board | 2 +- src/drivers/roboclaw/RoboClaw.cpp | 60 +++++++++++++------ src/drivers/roboclaw/RoboClaw.hpp | 22 +++++-- .../RoverPositionControl.cpp | 4 +- 4 files changed, 62 insertions(+), 26 deletions(-) diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index c9300dba0703..80e45a0074c8 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -15,7 +15,7 @@ CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y -CONFIG_COMMON_DISTANCE_SENSOR=y +CONFIG_COMMON_DISTANCE_SENSOR=n CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_ROBOCLAW=y CONFIG_DRIVERS_GPIO_MCP23009=y diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 669e7bcb2585..83e39fe01f50 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -136,16 +136,28 @@ RoboClaw::~RoboClaw() close(_uart); } +void +RoboClaw::vehicle_control_poll() +{ + if (_vehicle_thrust_setpoint_sub.updated()) { + _vehicle_thrust_setpoint_sub.copy(&vehicle_thrust_setpoint); + } + if (_vehicle_torque_setpoint_sub.updated()) { + _vehicle_torque_setpoint_sub.copy(&vehicle_torque_setpoint); + } +} + + void RoboClaw::taskMain() { // Make sure the Roboclaw is actually connected, so I don't just spam errors if it's not. - uint8_t rbuff[6]; - int err_code = _transaction(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true); + // uint8_t rbuff[6]; + // int err_code = _transaction(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true); - if (err_code <= 0) { - PX4_ERR("Unable to connect to Roboclaw. Shutting down Roboclaw driver."); - return; - } + // if (err_code <= 0) { + // PX4_ERR("Unable to connect to Roboclaw. Shutting down Roboclaw driver."); + // return; + // } // This main loop performs two different tasks, asynchronously: // - Send actuator_controls_0 to the Roboclaw as soon as they are available @@ -155,6 +167,9 @@ void RoboClaw::taskMain() // It is updated at the end of every loop. Sometimes, if the actuator_controls_0 message came in right before // I should have read the encoders, waitTime will be 0. This is fine. When waitTime is 0, poll() will return // immediately with a timeout. (Or possibly with a message, if one happened to be available at that exact moment) + + // printf("i am in main"); + int waitTime = 100_ms; _actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0)); @@ -173,8 +188,13 @@ void RoboClaw::taskMain() while (!taskShouldExit) { + // printf("i am running, thrust %f \n", (double)vehicle_thrust_setpoint.xyz[0]); + + int pret = poll(fds, sizeof(fds) / sizeof(pollfd), waitTime / 1000); + vehicle_control_poll(); + if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(parameter_update), _paramSub, &_paramUpdate); _parameters_update(); @@ -189,12 +209,14 @@ void RoboClaw::taskMain() || _actuatorArmed.force_failsafe; if (disarmed) { + // printf("i am disarmed \n"); setMotorDutyCycle(MOTOR_1, 0.f); setMotorDutyCycle(MOTOR_2, 0.f); } else { - const float throttle = _actuatorControls.control[actuator_controls_s::INDEX_THROTTLE]; - const float yaw = _actuatorControls.control[actuator_controls_s::INDEX_YAW]; + const float yaw = (double)vehicle_thrust_setpoint.xyz[0]; //temporary change + // printf("thrust %f\n", (double)throttle); + const float throttle = (double)vehicle_torque_setpoint.xyz[2]; const float scale = 0.3f; setMotorDutyCycle(MOTOR_1, (throttle - yaw) * scale); setMotorDutyCycle(MOTOR_2, (throttle + yaw) * scale); @@ -485,7 +507,7 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, // An error code of 0 means that select timed out, which is how the Roboclaw indicates an error. if (err_code <= 0) { - printf("select error: %d\n", err_code); + // printf("select error: %d\n", err_code); return err_code; } @@ -493,7 +515,7 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, //printf("Read: %d\n", err_code); if (err_code <= 0) { - printf("read error: %d\n", err_code); + // printf("read error: %d\n", err_code); return err_code; } else { @@ -560,17 +582,17 @@ void RoboClaw::_parameters_update() _parameters.serial_baud_rate = B2400; break; - case 9600: - _parameters.serial_baud_rate = B9600; - break; + // case 9600: + // _parameters.serial_baud_rate = B9600; + // break; - case 19200: - _parameters.serial_baud_rate = B19200; - break; + // case 19200: + // _parameters.serial_baud_rate = B19200; + // break; - case 38400: - _parameters.serial_baud_rate = B38400; - break; + // case 38400: + // _parameters.serial_baud_rate = B38400; + // break; case 57600: _parameters.serial_baud_rate = B57600; diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index 0a92d07f3993..998cdcabe8b7 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -47,15 +47,20 @@ #include #include #include +#include +#include +#include +#include + #include #include #include #include #include -#include -#include -#include -#include +#include +#include +#include + /** * This is a driver for the RoboClaw motor controller @@ -205,6 +210,14 @@ class RoboClaw int _paramSub{-1}; parameter_update_s _paramUpdate; + uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)}; + uORB::Subscription _vehicle_torque_setpoint_sub{ORB_ID(vehicle_torque_setpoint)}; + + vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; + vehicle_torque_setpoint_s vehicle_torque_setpoint{}; + + + uORB::PublicationMulti _wheelEncodersAdv[2] { ORB_ID(wheel_encoders), ORB_ID(wheel_encoders)}; wheel_encoders_s _wheelEncoderMsg[2]; @@ -213,6 +226,7 @@ class RoboClaw int32_t _motorSpeeds[2] {0, 0}; void _parameters_update(); + void vehicle_control_poll(); static uint16_t _calcCRC(const uint8_t *buf, size_t n, uint16_t init = 0); int _sendUnsigned7Bit(e_command command, float data); diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 24909280e99c..1c80808ec011 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -113,7 +113,7 @@ RoverPositionControl::vehicle_control_mode_poll() void RoverPositionControl::manual_control_setpoint_poll() { - if (_control_mode.flag_control_manual_enabled) { + if (true) { if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { float dt = math::constrain(hrt_elapsed_time(&_manual_setpoint_last_called) * 1e-6f, 0.0002f, 0.04f); @@ -431,7 +431,7 @@ RoverPositionControl::Run() matrix::Vector2d current_position(_global_pos.lat, _global_pos.lon); matrix::Vector3f current_velocity(_local_pos.vx, _local_pos.vy, _local_pos.vz); - if (!_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) { + if (false) { if (control_position(current_position, ground_speed, _pos_sp_triplet)) { From 7682456d3b4ecdcfddaee9c84016dfe798d31f75 Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Wed, 11 Oct 2023 15:00:35 +0200 Subject: [PATCH 06/27] Roboclaw: Replaced setDutyCycle with setMotorSpeed to allow for encoder data to get through (added support for encoders) --- src/drivers/roboclaw/RoboClaw.cpp | 271 +++++++++++++++++++++++++----- src/drivers/roboclaw/RoboClaw.hpp | 35 +++- 2 files changed, 259 insertions(+), 47 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 83e39fe01f50..0107cde82b2b 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -74,6 +74,9 @@ // Number of bytes for commands 18 and 19, read speeds. #define ENCODER_SPEED_MESSAGE_SIZE 7 +// Number of bytes for command 90, read reads status of the roboclaw. +#define CMD_READ_STATUS_MESSAGE_SIZE 6 + bool RoboClaw::taskShouldExit = false; using namespace time_literals; @@ -126,7 +129,7 @@ RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam): FD_SET(_uart, &_uart_set); // setup default settings, reset encoders - //resetEncoders(); + resetEncoders(); } RoboClaw::~RoboClaw() @@ -151,13 +154,15 @@ RoboClaw::vehicle_control_poll() void RoboClaw::taskMain() { // Make sure the Roboclaw is actually connected, so I don't just spam errors if it's not. - // uint8_t rbuff[6]; - // int err_code = _transaction(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true); + uint8_t rbuff[CMD_READ_STATUS_MESSAGE_SIZE]; + RoboClawError err_code = _validate_connection(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true); - // if (err_code <= 0) { - // PX4_ERR("Unable to connect to Roboclaw. Shutting down Roboclaw driver."); - // return; - // } + if (err_code != RoboClawError::Success) { + PX4_ERR("Shutting down Roboclaw driver."); + return; + } else { + PX4_ERR("Successfully connected"); + } // This main loop performs two different tasks, asynchronously: // - Send actuator_controls_0 to the Roboclaw as soon as they are available @@ -170,7 +175,11 @@ void RoboClaw::taskMain() // printf("i am in main"); - int waitTime = 100_ms; + int waitTime = 10_ms; + + // uint64_t encoderTaskLastRun = 0; + + uint64_t actuatorsLastWritten = 0; _actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0)); orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms); @@ -186,6 +195,10 @@ void RoboClaw::taskMain() fds[2].fd = _armedSub; fds[2].events = POLLIN; + memset((void *) &_wheelEncoderMsg[0], 0, sizeof(_wheelEncoderMsg)); + _wheelEncoderMsg[0].pulses_per_rev = _parameters.counts_per_rev; + _wheelEncoderMsg[1].pulses_per_rev = _parameters.counts_per_rev; + while (!taskShouldExit) { // printf("i am running, thrust %f \n", (double)vehicle_thrust_setpoint.xyz[0]); @@ -193,7 +206,13 @@ void RoboClaw::taskMain() int pret = poll(fds, sizeof(fds) / sizeof(pollfd), waitTime / 1000); - vehicle_control_poll(); + bool actuators_timeout = int(hrt_absolute_time() - actuatorsLastWritten) > 2000 * _parameters.actuator_write_period_ms; + + + // vehicle_control_poll(); + // readEncoder(); + + // printStatus(); if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(parameter_update), _paramSub, &_paramUpdate); @@ -201,7 +220,7 @@ void RoboClaw::taskMain() } // No timeout, update on either the actuator controls or the armed state - if (pret > 0 && (fds[1].revents & POLLIN || fds[2].revents & POLLIN)) { + if (pret > 0 && (fds[1].revents & POLLIN || fds[2].revents & POLLIN || actuators_timeout) ) { //temporary orb_copy(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuatorControls); orb_copy(ORB_ID(actuator_armed), _armedSub, &_actuatorArmed); @@ -210,20 +229,44 @@ void RoboClaw::taskMain() if (disarmed) { // printf("i am disarmed \n"); - setMotorDutyCycle(MOTOR_1, 0.f); - setMotorDutyCycle(MOTOR_2, 0.f); + PX4_ERR("Not armed so im not driving"); + setMotorSpeed(MOTOR_1, 0.f); + setMotorSpeed(MOTOR_2, 0.f); } else { + PX4_ERR("Armed so I can be driving"); + vehicle_control_poll(); const float yaw = (double)vehicle_thrust_setpoint.xyz[0]; //temporary change // printf("thrust %f\n", (double)throttle); const float throttle = (double)vehicle_torque_setpoint.xyz[2]; const float scale = 0.3f; - setMotorDutyCycle(MOTOR_1, (throttle - yaw) * scale); - setMotorDutyCycle(MOTOR_2, (throttle + yaw) * scale); + setMotorSpeed(MOTOR_1, (throttle - yaw) * scale); + setMotorSpeed(MOTOR_2, (throttle + yaw) * scale); + } + + actuatorsLastWritten = hrt_absolute_time(); + + } else { + PX4_ERR("im am going to try to readEncoder"); + if (readEncoder() > 0) { + + for (int i = 0; i < 2; i++) { + _wheelEncoderMsg[i].timestamp = encoderTaskLastRun; + + _wheelEncoderMsg[i].encoder_position = _encoderCounts[i]; + _wheelEncoderMsg[i].speed = _motorSpeeds[i]; + + _wheelEncodersAdv[i].publish(_wheelEncoderMsg[i]); + } + + } else { + PX4_ERR("Error reading encoders"); } + } } + orb_unsubscribe(_actuatorsSub); orb_unsubscribe(_armedSub); orb_unsubscribe(_paramSub); @@ -244,10 +287,13 @@ int RoboClaw::readEncoder() for (int retry = 0; retry < TIMEOUT_RETRIES && !success; retry++) { success = _transaction(CMD_READ_BOTH_ENCODERS, nullptr, 0, &rbuff_pos[0], ENCODER_MESSAGE_SIZE, false, true) == ENCODER_MESSAGE_SIZE; + PX4_ERR("success 1 %f", (double)success); success = success && _transaction(CMD_READ_SPEED_1, nullptr, 0, &rbuff_speed[0], ENCODER_SPEED_MESSAGE_SIZE, false, true) == ENCODER_SPEED_MESSAGE_SIZE; + PX4_ERR("success 2 %f", (double)success); success = success && _transaction(CMD_READ_SPEED_2, nullptr, 0, &rbuff_speed[4], ENCODER_SPEED_MESSAGE_SIZE, false, true) == ENCODER_SPEED_MESSAGE_SIZE; + PX4_ERR("success 3 %f", (double)success); } if (!success) { @@ -296,9 +342,9 @@ int RoboClaw::readEncoder() return 1; } -void RoboClaw::printStatus(char *string, size_t n) +void RoboClaw::printStatus() { - snprintf(string, n, "pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n", + PX4_ERR("pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n", double(getMotorPosition(MOTOR_1)), double(getMotorSpeed(MOTOR_1)), double(getMotorPosition(MOTOR_2)), @@ -446,6 +492,143 @@ uint16_t RoboClaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init) return crc; } +RoboClaw::RoboClawError RoboClaw::_validate_connection(e_command cmd, uint8_t *wbuff, size_t wbytes, uint8_t *rbuff, size_t rbytes, bool send_checksum, bool recv_checksum) +{ + + // Construct the packet to be sent. + uint8_t buf[wbytes + 4]; + buf[0] = (uint8_t) _parameters.address; + buf[1] = cmd; + + + RoboClawError err_code = writeToDevice(cmd, wbuff, wbytes, send_checksum, buf); + + if (err_code != RoboClawError::Success) { + printError(err_code); + return err_code; + } + + err_code = readFromDevice(rbuff, rbytes, recv_checksum, buf); + + if (err_code != RoboClawError::Success) { + printError(err_code); + return err_code; + } + + return RoboClawError::Success; +} + +RoboClaw::RoboClawError RoboClaw::writeToDevice(e_command cmd, uint8_t *wbuff, size_t wbytes, bool send_checksum, uint8_t *buf) +{ + + tcflush(_uart, TCIOFLUSH); // flush the buffers + + // // Construct the packet to be sent. + // uint8_t buf[wbytes + 4]; + // buf[0] = (uint8_t) _parameters.address; + // buf[1] = cmd; + + if (wbuff) { + memcpy(&buf[2], wbuff, wbytes); + } + + // Compute and append checksum if necessary. + wbytes = wbytes + (send_checksum ? 4 : 2); + + if (send_checksum) { + uint16_t sum = _calcCRC(buf, wbytes - 2); + buf[wbytes - 2] = (sum >> 8) & 0xFF; + buf[wbytes - 1] = sum & 0xFFu; + } + + // Write data to the device. + int count = write(_uart, buf, wbytes); + + // Error checking for the write operation. + if (count < (int) wbytes) { // Did not successfully send all bytes. + PX4_ERR("Only wrote %d out of %zu bytes", count, wbytes); + return RoboClawError::WriteError; + } + + return RoboClawError::Success; // Return success if write operation is successful +} + +RoboClaw::RoboClawError RoboClaw::readFromDevice(uint8_t *rbuff, size_t rbytes, bool recv_checksum, uint8_t *buf) +{ + + uint8_t *rbuff_curr = rbuff; + size_t bytes_read = 0; + + while (bytes_read < rbytes) { + + _uart_timeout.tv_sec = 0; + _uart_timeout.tv_usec = TIMEOUT_US; + + int select_status = select(_uart + 1, &_uart_set, nullptr, nullptr, &_uart_timeout); + + if (select_status <= 0) { + // Handle select error or timeout here + return RoboClawError::ReadTimeout; + } + + int err_code = read(_uart, rbuff_curr, rbytes - bytes_read); + + if (err_code <= 0) { + // Handle read error here + return RoboClawError::ReadError; + } + + bytes_read += err_code; + rbuff_curr += err_code; + } + + if (recv_checksum) { + + if (bytes_read < 2) { + return RoboClawError::ChecksumError; // or an equivalent error code of your choosing + } + + uint16_t checksum_calc = _calcCRC(buf, 2); // assuming buf contains address and command + checksum_calc = _calcCRC(rbuff, bytes_read - 2, checksum_calc); + uint16_t checksum_recv = (rbuff[bytes_read - 2] << 8) + rbuff[bytes_read - 1]; + + if (checksum_calc != checksum_recv) { + return RoboClawError::ChecksumError; + } + } + + + return RoboClawError::Success; // Return success if everything went well +} + +void RoboClaw::printError(RoboClawError error) +{ + + switch (error) { + case RoboClawError::Success: + // Optionally print success message or do nothing + break; + // case RoboClawError::InvalidUartFileDescriptor: + // PX4_ERR("Invalid UART file descriptor"); + // break; + case RoboClawError::WriteError: + PX4_ERR("Failed to write all bytes to the device"); + break; + case RoboClawError::ReadError: + PX4_ERR("Failed to read from the device"); + break; + case RoboClawError::ReadTimeout: + PX4_ERR("Timeout while reading from the device"); + break; + // Add more cases as needed for other error codes + default: + PX4_ERR("Unknown error code"); + } +} + + + + int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, uint8_t *rbuff, size_t rbytes, bool send_checksum, bool recv_checksum) { @@ -470,24 +653,22 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, buf[wbytes - 1] = sum & 0xFFu; } - // printf("Write: "); + // int count = write(_uart, buf, wbytes); - // for (size_t i = 0; i < wbytes; i++) { - // printf("%X ", buf[i]); - // } + int count = write(_uart, buf, wbytes); + PX4_INFO("wrote this many bytes: %d", count); - // printf("\n"); + PX4_INFO("Written data: "); + for (int i = 0; i < count; ++i) { + PX4_INFO("0x%08X (%d)", buf[i], buf[i]); + } - int count = write(_uart, buf, wbytes); if (count < (int) wbytes) { // Did not successfully send all bytes. PX4_ERR("Only wrote %d out of %zu bytes", count, wbytes); return -1; } - // else { - // printf("Successfully wrote %d of %zu bytes\n", count, wbytes); - // } // READ @@ -500,6 +681,8 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, //printf("Read: \n"); while (bytes_read < rbytes) { // select(...) may change this timeout struct (because it is not const). So I reset it every time. + PX4_INFO("I am in the read loop."); + _uart_timeout.tv_sec = 0; _uart_timeout.tv_usec = TIMEOUT_US; err_code = select(_uart + 1, &_uart_set, nullptr, nullptr, &_uart_timeout); @@ -507,15 +690,19 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, // An error code of 0 means that select timed out, which is how the Roboclaw indicates an error. if (err_code <= 0) { - // printf("select error: %d\n", err_code); + PX4_INFO("I timed out lol"); return err_code; } + PX4_INFO("Attempting to read data..."); err_code = read(_uart, rbuff_curr, rbytes - bytes_read); - //printf("Read: %d\n", err_code); + PX4_INFO("Read call returned: %d", err_code); + + for (int i = 0; i < err_code; i++) { + PX4_ERR("Received byte printt %d: 0x%08X (%d)", i, rbuff_curr[i], rbuff_curr[i]); + } if (err_code <= 0) { - // printf("read error: %d\n", err_code); return err_code; } else { @@ -524,14 +711,6 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, } } - // printf("Read: "); - - // for (size_t i = 0; i < bytes_read; i++) { - // printf("%X ", rbuff[i]); - // } - - // printf("\n"); - //TODO: Clean up this mess of IFs and returns if (recv_checksum) { @@ -545,7 +724,7 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, checksum_calc = _calcCRC(rbuff, bytes_read - 2, checksum_calc); uint16_t checksum_recv = (rbuff[bytes_read - 2] << 8) + rbuff[bytes_read - 1]; - //printf("crc calc: %X crc rec: %X\n", checksum_calc, checksum_recv); + // If the checksum is correct and the data has not been corrupted, it will return the bytes read if (checksum_calc == checksum_recv) { return bytes_read; @@ -582,17 +761,17 @@ void RoboClaw::_parameters_update() _parameters.serial_baud_rate = B2400; break; - // case 9600: - // _parameters.serial_baud_rate = B9600; - // break; + case 9600: + _parameters.serial_baud_rate = B9600; + break; - // case 19200: - // _parameters.serial_baud_rate = B19200; - // break; + case 19200: + _parameters.serial_baud_rate = B19200; + break; - // case 38400: - // _parameters.serial_baud_rate = B38400; - // break; + case 38400: + _parameters.serial_baud_rate = B38400; + break; case 57600: _parameters.serial_baud_rate = B57600; diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index 998cdcabe8b7..a154f3c9b64d 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -84,6 +84,17 @@ class RoboClaw MOTOR_2 }; + /** error handeling*/ + enum class RoboClawError { + Success, + WriteError, + ReadError, + ChecksumError, + ChecksumMismatch, + UnexpectedError, + ReadTimeout + }; + /** * constructor * @param deviceName the name of the @@ -143,7 +154,8 @@ class RoboClaw /** * print status */ - void printStatus(char *string, size_t n); + void printStatus(); + private: @@ -180,6 +192,7 @@ class RoboClaw CMD_READ_STATUS = 90 }; + struct { speed_t serial_baud_rate; int32_t counts_per_rev; @@ -233,6 +246,23 @@ class RoboClaw int _sendSigned16Bit(e_command command, float data); int _sendNothing(e_command); + + /** + * print status + */ + RoboClawError writeToDevice(e_command cmd, uint8_t *wbuff, size_t wbytes, bool send_checksum, uint8_t *buf); + + /** + * print status + */ + RoboClawError readFromDevice(uint8_t *rbuff, size_t rbytes, bool recv_checksum, uint8_t *buf); + + /** + * print status + */ + void printError(RoboClawError err_code); + + /** * Perform a round-trip write and read. * @@ -256,4 +286,7 @@ class RoboClaw */ int _transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, uint8_t *rbuff, size_t rbytes, bool send_checksum = true, bool recv_checksum = false); + + RoboClawError _validate_connection(e_command cmd, uint8_t *wbuff, size_t wbytes, + uint8_t *rbuff, size_t rbytes, bool send_checksum = true, bool recv_checksum = false); }; From 2dc976e761bcf77e0621ce224524d4ac4dddee63 Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Wed, 11 Oct 2023 15:40:41 +0200 Subject: [PATCH 07/27] Roboclaw: Initial cleanup, next commit will be refactor removing the duplicated write and read functions --- src/drivers/roboclaw/RoboClaw.cpp | 42 +++---------------------------- 1 file changed, 3 insertions(+), 39 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 0107cde82b2b..72d6d9a18323 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -161,7 +161,7 @@ void RoboClaw::taskMain() PX4_ERR("Shutting down Roboclaw driver."); return; } else { - PX4_ERR("Successfully connected"); + PX4_INFO("Successfully connected"); } // This main loop performs two different tasks, asynchronously: @@ -177,7 +177,7 @@ void RoboClaw::taskMain() int waitTime = 10_ms; - // uint64_t encoderTaskLastRun = 0; + uint64_t encoderTaskLastRun = 0; uint64_t actuatorsLastWritten = 0; @@ -208,12 +208,6 @@ void RoboClaw::taskMain() bool actuators_timeout = int(hrt_absolute_time() - actuatorsLastWritten) > 2000 * _parameters.actuator_write_period_ms; - - // vehicle_control_poll(); - // readEncoder(); - - // printStatus(); - if (fds[0].revents & POLLIN) { orb_copy(ORB_ID(parameter_update), _paramSub, &_paramUpdate); _parameters_update(); @@ -229,12 +223,10 @@ void RoboClaw::taskMain() if (disarmed) { // printf("i am disarmed \n"); - PX4_ERR("Not armed so im not driving"); setMotorSpeed(MOTOR_1, 0.f); setMotorSpeed(MOTOR_2, 0.f); } else { - PX4_ERR("Armed so I can be driving"); vehicle_control_poll(); const float yaw = (double)vehicle_thrust_setpoint.xyz[0]; //temporary change // printf("thrust %f\n", (double)throttle); @@ -247,7 +239,7 @@ void RoboClaw::taskMain() actuatorsLastWritten = hrt_absolute_time(); } else { - PX4_ERR("im am going to try to readEncoder"); + if (readEncoder() > 0) { for (int i = 0; i < 2; i++) { @@ -287,17 +279,13 @@ int RoboClaw::readEncoder() for (int retry = 0; retry < TIMEOUT_RETRIES && !success; retry++) { success = _transaction(CMD_READ_BOTH_ENCODERS, nullptr, 0, &rbuff_pos[0], ENCODER_MESSAGE_SIZE, false, true) == ENCODER_MESSAGE_SIZE; - PX4_ERR("success 1 %f", (double)success); success = success && _transaction(CMD_READ_SPEED_1, nullptr, 0, &rbuff_speed[0], ENCODER_SPEED_MESSAGE_SIZE, false, true) == ENCODER_SPEED_MESSAGE_SIZE; - PX4_ERR("success 2 %f", (double)success); success = success && _transaction(CMD_READ_SPEED_2, nullptr, 0, &rbuff_speed[4], ENCODER_SPEED_MESSAGE_SIZE, false, true) == ENCODER_SPEED_MESSAGE_SIZE; - PX4_ERR("success 3 %f", (double)success); } if (!success) { - PX4_ERR("Error reading encoders"); return -1; } @@ -523,11 +511,6 @@ RoboClaw::RoboClawError RoboClaw::writeToDevice(e_command cmd, uint8_t *wbuff, s tcflush(_uart, TCIOFLUSH); // flush the buffers - // // Construct the packet to be sent. - // uint8_t buf[wbytes + 4]; - // buf[0] = (uint8_t) _parameters.address; - // buf[1] = cmd; - if (wbuff) { memcpy(&buf[2], wbuff, wbytes); } @@ -606,11 +589,7 @@ void RoboClaw::printError(RoboClawError error) switch (error) { case RoboClawError::Success: - // Optionally print success message or do nothing break; - // case RoboClawError::InvalidUartFileDescriptor: - // PX4_ERR("Invalid UART file descriptor"); - // break; case RoboClawError::WriteError: PX4_ERR("Failed to write all bytes to the device"); break; @@ -620,7 +599,6 @@ void RoboClaw::printError(RoboClawError error) case RoboClawError::ReadTimeout: PX4_ERR("Timeout while reading from the device"); break; - // Add more cases as needed for other error codes default: PX4_ERR("Unknown error code"); } @@ -656,13 +634,6 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, // int count = write(_uart, buf, wbytes); int count = write(_uart, buf, wbytes); - PX4_INFO("wrote this many bytes: %d", count); - - PX4_INFO("Written data: "); - for (int i = 0; i < count; ++i) { - PX4_INFO("0x%08X (%d)", buf[i], buf[i]); - } - if (count < (int) wbytes) { // Did not successfully send all bytes. PX4_ERR("Only wrote %d out of %zu bytes", count, wbytes); @@ -690,17 +661,10 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, // An error code of 0 means that select timed out, which is how the Roboclaw indicates an error. if (err_code <= 0) { - PX4_INFO("I timed out lol"); return err_code; } - PX4_INFO("Attempting to read data..."); err_code = read(_uart, rbuff_curr, rbytes - bytes_read); - PX4_INFO("Read call returned: %d", err_code); - - for (int i = 0; i < err_code; i++) { - PX4_ERR("Received byte printt %d: 0x%08X (%d)", i, rbuff_curr[i], rbuff_curr[i]); - } if (err_code <= 0) { return err_code; From 18416e803e8476d8521365af9c623afc6708643c Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Wed, 11 Oct 2023 16:51:14 +0200 Subject: [PATCH 08/27] Roboclaw: rough refactor, removed repetitive code, simplified and clarified logic and error handeling --- src/drivers/roboclaw/RoboClaw.cpp | 123 ++++-------------------------- src/drivers/roboclaw/RoboClaw.hpp | 2 - 2 files changed, 13 insertions(+), 112 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 72d6d9a18323..9c9e3344b201 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -155,9 +155,9 @@ void RoboClaw::taskMain() { // Make sure the Roboclaw is actually connected, so I don't just spam errors if it's not. uint8_t rbuff[CMD_READ_STATUS_MESSAGE_SIZE]; - RoboClawError err_code = _validate_connection(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true); + int err_code = _transaction(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true); - if (err_code != RoboClawError::Success) { + if (err_code < 0) { PX4_ERR("Shutting down Roboclaw driver."); return; } else { @@ -278,11 +278,11 @@ int RoboClaw::readEncoder() for (int retry = 0; retry < TIMEOUT_RETRIES && !success; retry++) { success = _transaction(CMD_READ_BOTH_ENCODERS, nullptr, 0, &rbuff_pos[0], ENCODER_MESSAGE_SIZE, false, - true) == ENCODER_MESSAGE_SIZE; + true) > 0; success = success && _transaction(CMD_READ_SPEED_1, nullptr, 0, &rbuff_speed[0], ENCODER_SPEED_MESSAGE_SIZE, false, - true) == ENCODER_SPEED_MESSAGE_SIZE; + true) > 0; success = success && _transaction(CMD_READ_SPEED_2, nullptr, 0, &rbuff_speed[4], ENCODER_SPEED_MESSAGE_SIZE, false, - true) == ENCODER_SPEED_MESSAGE_SIZE; + true) > 0; } if (!success) { @@ -480,7 +480,7 @@ uint16_t RoboClaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init) return crc; } -RoboClaw::RoboClawError RoboClaw::_validate_connection(e_command cmd, uint8_t *wbuff, size_t wbytes, uint8_t *rbuff, size_t rbytes, bool send_checksum, bool recv_checksum) +int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, uint8_t *rbuff, size_t rbytes, bool send_checksum, bool recv_checksum) { // Construct the packet to be sent. @@ -493,17 +493,21 @@ RoboClaw::RoboClawError RoboClaw::_validate_connection(e_command cmd, uint8_t *w if (err_code != RoboClawError::Success) { printError(err_code); - return err_code; + PX4_ERR("uhh 1"); + + return -1; } err_code = readFromDevice(rbuff, rbytes, recv_checksum, buf); if (err_code != RoboClawError::Success) { printError(err_code); - return err_code; + PX4_ERR("uhh 2"); + + return -1; } - return RoboClawError::Success; + return 1; } RoboClaw::RoboClawError RoboClaw::writeToDevice(e_command cmd, uint8_t *wbuff, size_t wbytes, bool send_checksum, uint8_t *buf) @@ -605,107 +609,6 @@ void RoboClaw::printError(RoboClawError error) } - - -int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, - uint8_t *rbuff, size_t rbytes, bool send_checksum, bool recv_checksum) -{ - int err_code = 0; - - // WRITE - - tcflush(_uart, TCIOFLUSH); // flush buffers - uint8_t buf[wbytes + 4]; - buf[0] = (uint8_t) _parameters.address; - buf[1] = cmd; - - if (wbuff) { - memcpy(&buf[2], wbuff, wbytes); - } - - wbytes = wbytes + (send_checksum ? 4 : 2); - - if (send_checksum) { - uint16_t sum = _calcCRC(buf, wbytes - 2); - buf[wbytes - 2] = (sum >> 8) & 0xFF; - buf[wbytes - 1] = sum & 0xFFu; - } - - // int count = write(_uart, buf, wbytes); - - int count = write(_uart, buf, wbytes); - - if (count < (int) wbytes) { // Did not successfully send all bytes. - PX4_ERR("Only wrote %d out of %zu bytes", count, wbytes); - return -1; - } - - - // READ - - - uint8_t *rbuff_curr = rbuff; - size_t bytes_read = 0; - - // select(...) returns as soon as even 1 byte is available. read(...) returns immediately, no matter how many - // bytes are available. I need to keep reading until I get the number of bytes I expect. - //printf("Read: \n"); - while (bytes_read < rbytes) { - // select(...) may change this timeout struct (because it is not const). So I reset it every time. - PX4_INFO("I am in the read loop."); - - _uart_timeout.tv_sec = 0; - _uart_timeout.tv_usec = TIMEOUT_US; - err_code = select(_uart + 1, &_uart_set, nullptr, nullptr, &_uart_timeout); - //printf("Select: %d\n", err_code); - - // An error code of 0 means that select timed out, which is how the Roboclaw indicates an error. - if (err_code <= 0) { - return err_code; - } - - err_code = read(_uart, rbuff_curr, rbytes - bytes_read); - - if (err_code <= 0) { - return err_code; - - } else { - bytes_read += err_code; - rbuff_curr += err_code; - } - } - - //TODO: Clean up this mess of IFs and returns - - if (recv_checksum) { - if (bytes_read < 2) { - return -1; - } - - // The checksum sent back by the roboclaw is calculated based on the address and command bytes as well - // as the data returned. - uint16_t checksum_calc = _calcCRC(buf, 2); - checksum_calc = _calcCRC(rbuff, bytes_read - 2, checksum_calc); - uint16_t checksum_recv = (rbuff[bytes_read - 2] << 8) + rbuff[bytes_read - 1]; - - // If the checksum is correct and the data has not been corrupted, it will return the bytes read - if (checksum_calc == checksum_recv) { - return bytes_read; - - } else { - return -10; - } - - } else { - if (bytes_read == 1 && rbuff[0] == 0xFF) { - return 1; - - } else { - return -11; - } - } -} - void RoboClaw::_parameters_update() { param_get(_param_handles.counts_per_rev, &_parameters.counts_per_rev); diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index a154f3c9b64d..f4df94c43e1b 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -287,6 +287,4 @@ class RoboClaw int _transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, uint8_t *rbuff, size_t rbytes, bool send_checksum = true, bool recv_checksum = false); - RoboClawError _validate_connection(e_command cmd, uint8_t *wbuff, size_t wbytes, - uint8_t *rbuff, size_t rbytes, bool send_checksum = true, bool recv_checksum = false); }; From 8d39d63d8ab0979685cf4b22838aaf3a4b2c0fae Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Thu, 12 Oct 2023 16:26:02 +0200 Subject: [PATCH 09/27] Roboclaw: Fixed issue when power cylcing the roboclaw where the driver would not connect --- src/drivers/roboclaw/RoboClaw.cpp | 82 +++++++++++++++++++++---------- src/drivers/roboclaw/RoboClaw.hpp | 8 +++ 2 files changed, 63 insertions(+), 27 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 9c9e3344b201..4ba2a0bb48b1 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -62,7 +62,7 @@ #define TIMEOUT_US 10500 // If a timeout occurs during serial communication, it will immediately try again this many times -#define TIMEOUT_RETRIES 1 +#define TIMEOUT_RETRIES 5 // 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. @@ -81,29 +81,55 @@ bool RoboClaw::taskShouldExit = false; using namespace time_literals; -RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam): - _uart(0), - _uart_set(), - _uart_timeout{.tv_sec = 0, .tv_usec = TIMEOUT_US}, - _actuatorsSub(-1), - _lastEncoderCount{0, 0}, - _encoderCounts{0, 0}, - _motorSpeeds{0, 0} +RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam) +{ + strncpy(_storedDeviceName, deviceName, sizeof(_storedDeviceName) - 1); + _storedDeviceName[sizeof(_storedDeviceName) - 1] = '\0'; // Ensure null-termination + + strncpy(_storedBaudRateParam, baudRateParam, sizeof(_storedBaudRateParam) - 1); + _storedBaudRateParam[sizeof(_storedBaudRateParam) - 1] = '\0'; // Ensure null-termination + + initialize(); +} +RoboClaw::~RoboClaw() { - _param_handles.actuator_write_period_ms = param_find("RBCLW_WRITE_PER"); + setMotorDutyCycle(MOTOR_1, 0.0); + setMotorDutyCycle(MOTOR_2, 0.0); + close(_uart); +} + +void +RoboClaw::initialize() { + + _uart = 0; + _uart_timeout = { .tv_sec = 0, .tv_usec = TIMEOUT_US }; + _actuatorsSub = -1; + _lastEncoderCount[0] = 0; + _lastEncoderCount[1] = 0; + _encoderCounts[0] = 0; + _encoderCounts[1] = 0; + _motorSpeeds[0] = 0; + _motorSpeeds[1] = 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"); - printf("Baudrate parameter: %s\n", baudRateParam); - _param_handles.serial_baud_rate = param_find(baudRateParam); + printf("Baudrate parameter: %s\n", _storedBaudRateParam); + _param_handles.serial_baud_rate = param_find(_storedBaudRateParam); _param_handles.address = param_find("RBCLW_ADDRESS"); _parameters_update(); + PX4_ERR("trying to open uart"); + // start serial port - _uart = open(deviceName, O_RDWR | O_NOCTTY); + _uart = open(_storedDeviceName, O_RDWR | O_NOCTTY); + + if (_uart < 0) { err(1, "could not open %s", _storedDeviceName); } + + // PX4_ERR("uart connection %f", (double)_uart); - if (_uart < 0) { err(1, "could not open %s", deviceName); } int ret = 0; struct termios uart_config {}; @@ -130,15 +156,10 @@ RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam): // setup default settings, reset encoders resetEncoders(); -} -RoboClaw::~RoboClaw() -{ - setMotorDutyCycle(MOTOR_1, 0.0); - setMotorDutyCycle(MOTOR_2, 0.0); - close(_uart); } + void RoboClaw::vehicle_control_poll() { @@ -175,7 +196,7 @@ void RoboClaw::taskMain() // printf("i am in main"); - int waitTime = 10_ms; + int waitTime = 100_ms; uint64_t encoderTaskLastRun = 0; @@ -493,8 +514,6 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, uint8_t if (err_code != RoboClawError::Success) { printError(err_code); - PX4_ERR("uhh 1"); - return -1; } @@ -502,8 +521,6 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, uint8_t if (err_code != RoboClawError::Success) { printError(err_code); - PX4_ERR("uhh 2"); - return -1; } @@ -529,6 +546,7 @@ RoboClaw::RoboClawError RoboClaw::writeToDevice(e_command cmd, uint8_t *wbuff, s } // Write data to the device. + int count = write(_uart, buf, wbytes); // Error checking for the write operation. @@ -555,10 +573,20 @@ RoboClaw::RoboClawError RoboClaw::readFromDevice(uint8_t *rbuff, size_t rbytes, if (select_status <= 0) { // Handle select error or timeout here - return RoboClawError::ReadTimeout; + PX4_ERR("error %f", (double)select_status); + + usleep(20000000); // 20 second delay + + PX4_ERR("Trying again to reconnect to RoboClaw"); + + // Reinitialize the roboclaw driver + initialize(); + + // return RoboClawError::ReadTimeout; + } - int err_code = read(_uart, rbuff_curr, rbytes - bytes_read); + int err_code = read(_uart, rbuff_curr, rbytes - bytes_read); if (err_code <= 0) { // Handle read error here diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index f4df94c43e1b..abbb33985770 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -51,6 +51,7 @@ #include #include #include +#include #include #include @@ -110,6 +111,8 @@ class RoboClaw */ virtual ~RoboClaw(); + void initialize(); + /** * @return position of a motor, rev */ @@ -159,6 +162,11 @@ class RoboClaw private: + char _storedDeviceName[256]; // Adjust size as necessary + char _storedBaudRateParam[256]; // Adjust size as necessary + + int _timeout_counter = 0; + // commands // We just list the commands we want from the manual here. enum e_command { From 51ed2aae75098379a8be811b407f02d9ee4fc941 Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Fri, 10 Nov 2023 10:43:44 +0100 Subject: [PATCH 10/27] Roboclaw: Integrated OutputModuleInterface including a large code refactor --- src/drivers/roboclaw/RoboClaw.cpp | 653 ++++++++++++------------------ src/drivers/roboclaw/RoboClaw.hpp | 197 ++++----- 2 files changed, 327 insertions(+), 523 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 4ba2a0bb48b1..341d94655406 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2023 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 @@ -41,6 +41,7 @@ * */ + #include "RoboClaw.hpp" #include #include @@ -74,321 +75,235 @@ // Number of bytes for commands 18 and 19, read speeds. #define ENCODER_SPEED_MESSAGE_SIZE 7 -// Number of bytes for command 90, read reads status of the roboclaw. -#define CMD_READ_STATUS_MESSAGE_SIZE 6 - -bool RoboClaw::taskShouldExit = false; - using namespace time_literals; -RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam) +RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam) : + // ModuleParams(nullptr), + OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default) + // ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) { strncpy(_storedDeviceName, deviceName, sizeof(_storedDeviceName) - 1); _storedDeviceName[sizeof(_storedDeviceName) - 1] = '\0'; // Ensure null-termination strncpy(_storedBaudRateParam, baudRateParam, sizeof(_storedBaudRateParam) - 1); _storedBaudRateParam[sizeof(_storedBaudRateParam) - 1] = '\0'; // Ensure null-termination - - initialize(); } RoboClaw::~RoboClaw() { - setMotorDutyCycle(MOTOR_1, 0.0); - setMotorDutyCycle(MOTOR_2, 0.0); - close(_uart); + close(_uart_fd); } -void -RoboClaw::initialize() { +int RoboClaw::init() { + _uart_fd_timeout = { .tv_sec = 0, .tv_usec = TIMEOUT_US }; - _uart = 0; - _uart_timeout = { .tv_sec = 0, .tv_usec = TIMEOUT_US }; - _actuatorsSub = -1; - _lastEncoderCount[0] = 0; - _lastEncoderCount[1] = 0; - _encoderCounts[0] = 0; - _encoderCounts[1] = 0; - _motorSpeeds[0] = 0; - _motorSpeeds[1] = 0; + int32_t baud_rate_parameter_value{0}; + int32_t baud_rate_posix{0}; + param_get(param_find(_storedBaudRateParam), &baud_rate_parameter_value); - _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"); - printf("Baudrate parameter: %s\n", _storedBaudRateParam); - _param_handles.serial_baud_rate = param_find(_storedBaudRateParam); - _param_handles.address = param_find("RBCLW_ADDRESS"); + switch (baud_rate_parameter_value) { + case 0: // Auto + default: + PX4_ERR("Please configure the port's baud_rate_parameter_value"); + break; - _parameters_update(); + case 2400: + baud_rate_posix = B2400; + break; - PX4_ERR("trying to open uart"); + case 9600: + baud_rate_posix = B9600; + break; - // start serial port - _uart = open(_storedDeviceName, O_RDWR | O_NOCTTY); + case 19200: + baud_rate_posix = B19200; + break; + + case 38400: + baud_rate_posix = B38400; + break; + + case 57600: + baud_rate_posix = B57600; + break; + + case 115200: + baud_rate_posix = B115200; + break; - if (_uart < 0) { err(1, "could not open %s", _storedDeviceName); } + case 230400: + baud_rate_posix = B230400; + break; - // PX4_ERR("uart connection %f", (double)_uart); + case 460800: + baud_rate_posix = B460800; + break; + } + // start serial port + _uart_fd = open(_storedDeviceName, O_RDWR | O_NOCTTY); + + if (_uart_fd < 0) { err(1, "could not open %s", _storedDeviceName); } int ret = 0; struct termios uart_config {}; - ret = tcgetattr(_uart, &uart_config); + ret = tcgetattr(_uart_fd, &uart_config); if (ret < 0) { err(1, "failed to get attr"); } uart_config.c_oflag &= ~ONLCR; // no CR for every LF uart_config.c_cflag &= ~CRTSCTS; - ret = cfsetispeed(&uart_config, _parameters.serial_baud_rate); + // Set baud rate + ret = cfsetispeed(&uart_config, baud_rate_posix); if (ret < 0) { err(1, "failed to set input speed"); } - ret = cfsetospeed(&uart_config, _parameters.serial_baud_rate); - + ret = cfsetospeed(&uart_config, baud_rate_posix); if (ret < 0) { err(1, "failed to set output speed"); } - ret = tcsetattr(_uart, TCSANOW, &uart_config); - + ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); if (ret < 0) { err(1, "failed to set attr"); } - FD_ZERO(&_uart_set); - FD_SET(_uart, &_uart_set); - - // setup default settings, reset encoders - resetEncoders(); - -} - + FD_ZERO(&_uart_fd_set); + FD_SET(_uart_fd, &_uart_fd_set); -void -RoboClaw::vehicle_control_poll() -{ - if (_vehicle_thrust_setpoint_sub.updated()) { - _vehicle_thrust_setpoint_sub.copy(&vehicle_thrust_setpoint); - } - if (_vehicle_torque_setpoint_sub.updated()) { - _vehicle_torque_setpoint_sub.copy(&vehicle_torque_setpoint); - } -} - - -void RoboClaw::taskMain() -{ // Make sure the Roboclaw is actually connected, so I don't just spam errors if it's not. - uint8_t rbuff[CMD_READ_STATUS_MESSAGE_SIZE]; - int err_code = _transaction(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true); + uint8_t rbuff[6]; // Number of bytes for command 90 status message, read reads status of the roboclaw. + int err_code = receiveTransaction(CMD_READ_STATUS, &rbuff[0], sizeof(rbuff)); - if (err_code < 0) { + if (err_code <= 0) { PX4_ERR("Shutting down Roboclaw driver."); - return; + return PX4_ERROR; } else { PX4_INFO("Successfully connected"); + /* Schedule a cycle to start things. */ + _successfully_connected = true; + return PX4_OK; } +} - // This main loop performs two different tasks, asynchronously: - // - Send actuator_controls_0 to the Roboclaw as soon as they are available - // - Read the encoder values at a constant rate - // To do this, the timeout on the poll() function is used. - // waitTime is the amount of time left (int microseconds) until the next time I should read from the encoders. - // It is updated at the end of every loop. Sometimes, if the actuator_controls_0 message came in right before - // I should have read the encoders, waitTime will be 0. This is fine. When waitTime is 0, poll() will return - // immediately with a timeout. (Or possibly with a message, if one happened to be available at that exact moment) - - // printf("i am in main"); - - int waitTime = 100_ms; - - uint64_t encoderTaskLastRun = 0; - - uint64_t actuatorsLastWritten = 0; - - _actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0)); - orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms); - - _armedSub = orb_subscribe(ORB_ID(actuator_armed)); - _paramSub = orb_subscribe(ORB_ID(parameter_update)); - - pollfd fds[3]; - fds[0].fd = _paramSub; - fds[0].events = POLLIN; - fds[1].fd = _actuatorsSub; - fds[1].events = POLLIN; - fds[2].fd = _armedSub; - fds[2].events = POLLIN; - - memset((void *) &_wheelEncoderMsg[0], 0, sizeof(_wheelEncoderMsg)); - _wheelEncoderMsg[0].pulses_per_rev = _parameters.counts_per_rev; - _wheelEncoderMsg[1].pulses_per_rev = _parameters.counts_per_rev; - - while (!taskShouldExit) { - - // printf("i am running, thrust %f \n", (double)vehicle_thrust_setpoint.xyz[0]); +int RoboClaw::task_spawn(int argc, char *argv[]) +{ + const char *deviceName = argv[1]; + const char *baud_rate_parameter_value = argv[2]; + RoboClaw *instance = new RoboClaw(deviceName, baud_rate_parameter_value); - int pret = poll(fds, sizeof(fds) / sizeof(pollfd), waitTime / 1000); + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + // instance->ScheduleOnInterval(10_ms); // 100 Hz + instance->ScheduleNow(); + return PX4_OK; - bool actuators_timeout = int(hrt_absolute_time() - actuatorsLastWritten) > 2000 * _parameters.actuator_write_period_ms; + } else { + PX4_ERR("alloc failed"); + } - if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(parameter_update), _paramSub, &_paramUpdate); - _parameters_update(); - } + delete instance; + _object.store(nullptr); + _task_id = -1; - // No timeout, update on either the actuator controls or the armed state - if (pret > 0 && (fds[1].revents & POLLIN || fds[2].revents & POLLIN || actuators_timeout) ) { //temporary - orb_copy(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuatorControls); - orb_copy(ORB_ID(actuator_armed), _armedSub, &_actuatorArmed); + printf("Ending task_spawn"); - const bool disarmed = !_actuatorArmed.armed || _actuatorArmed.lockdown || _actuatorArmed.manual_lockdown - || _actuatorArmed.force_failsafe; + return PX4_ERROR; +} - if (disarmed) { - // printf("i am disarmed \n"); - setMotorSpeed(MOTOR_1, 0.f); - setMotorSpeed(MOTOR_2, 0.f); +bool RoboClaw::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) +{ + float left_motor_output = -((float)outputs[1] - 128.0f)/127.f; + float right_motor_output = ((float)outputs[0] - 128.0f)/127.f; - } else { - vehicle_control_poll(); - const float yaw = (double)vehicle_thrust_setpoint.xyz[0]; //temporary change - // printf("thrust %f\n", (double)throttle); - const float throttle = (double)vehicle_torque_setpoint.xyz[2]; - const float scale = 0.3f; - setMotorSpeed(MOTOR_1, (throttle - yaw) * scale); - setMotorSpeed(MOTOR_2, (throttle + yaw) * scale); - } + if(stop_motors){ + setMotorSpeed(MOTOR_1, 0.f); + setMotorSpeed(MOTOR_2, 0.f); + } else { + // PX4_ERR("Left and Right motor speed %f %f", (double)left_motor_output, (double)right_motor_output); + setMotorSpeed(MOTOR_1, left_motor_output); + setMotorSpeed(MOTOR_2, right_motor_output); + } + return true; +} - actuatorsLastWritten = hrt_absolute_time(); +void RoboClaw::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + _mixing_output.unregister(); + return; + } - } else { + _mixing_output.update(); - if (readEncoder() > 0) { + if(!_initialized){ + init(); + _initialized = true; + } - for (int i = 0; i < 2; i++) { - _wheelEncoderMsg[i].timestamp = encoderTaskLastRun; + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s pupdate; + _parameter_update_sub.copy(&pupdate); - _wheelEncoderMsg[i].encoder_position = _encoderCounts[i]; - _wheelEncoderMsg[i].speed = _motorSpeeds[i]; + updateParams(); + } - _wheelEncodersAdv[i].publish(_wheelEncoderMsg[i]); - } + _actuator_armed_sub.update(); - } else { - PX4_ERR("Error reading encoders"); - } + _mixing_output.updateSubscriptions(false); - } + if (readEncoder() < 0) { + PX4_ERR("Error reading encoders"); } - - orb_unsubscribe(_actuatorsSub); - orb_unsubscribe(_armedSub); - orb_unsubscribe(_paramSub); } int RoboClaw::readEncoder() { + uint8_t buffer_positon[ENCODER_MESSAGE_SIZE]; + uint8_t buffer_speed_right[ENCODER_SPEED_MESSAGE_SIZE]; + uint8_t buffer_speed_left[ENCODER_SPEED_MESSAGE_SIZE]; - uint8_t rbuff_pos[ENCODER_MESSAGE_SIZE]; - // 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 < TIMEOUT_RETRIES && !success; retry++) { - success = _transaction(CMD_READ_BOTH_ENCODERS, nullptr, 0, &rbuff_pos[0], ENCODER_MESSAGE_SIZE, false, - true) > 0; - success = success && _transaction(CMD_READ_SPEED_1, nullptr, 0, &rbuff_speed[0], ENCODER_SPEED_MESSAGE_SIZE, false, - true) > 0; - success = success && _transaction(CMD_READ_SPEED_2, nullptr, 0, &rbuff_speed[4], ENCODER_SPEED_MESSAGE_SIZE, false, - true) > 0; + if (receiveTransaction(CMD_READ_BOTH_ENCODERS, buffer_positon, ENCODER_MESSAGE_SIZE) < ENCODER_MESSAGE_SIZE) { + return -1; } - if (!success) { + if (receiveTransaction(CMD_READ_SPEED_1, buffer_speed_right, ENCODER_SPEED_MESSAGE_SIZE) < ENCODER_SPEED_MESSAGE_SIZE) { return -1; } - uint32_t count; - uint32_t speed; - uint8_t *count_bytes; - uint8_t *speed_bytes; - - for (int motor = 0; motor <= 1; motor++) { - count = 0; - speed = 0; - count_bytes = &rbuff_pos[motor * 4]; - speed_bytes = &rbuff_speed[motor * 4]; - - // Data from the roboclaw is big-endian. This converts the bytes to an integer, regardless of the - // endianness of the system this code is running on. - for (int byte = 0; byte < 4; byte++) { - count = (count << 8) + count_bytes[byte]; - speed = (speed << 8) + speed_bytes[byte]; - } - - // The Roboclaw stores encoder counts as unsigned 32-bit ints. This can overflow, especially when starting - // at 0 and moving backward. The Roboclaw has overflow flags for this, but in my testing, they don't seem - // to work. This code detects overflow manually. - // These diffs are the difference between the count I just read from the Roboclaw and the last - // count that was read from the roboclaw for this motor. fwd_diff assumes that the wheel moved forward, - // and rev_diff assumes it moved backward. If the motor actually moved forward, then rev_diff will be close - // to 2^32 (UINT32_MAX). If the motor actually moved backward, then fwd_diff will be close to 2^32. - // To detect and account for overflow, I just assume that the smaller diff is correct. - // Strictly speaking, if the wheel rotated more than 2^31 encoder counts since the last time I checked, this - // will be wrong. But that's 1.7 million revolutions, so it probably won't come up. - uint32_t fwd_diff = count - _lastEncoderCount[motor]; - uint32_t rev_diff = _lastEncoderCount[motor] - count; - // At this point, abs(diff) is always <= 2^31, so this cast from unsigned to signed is safe. - int32_t diff = fwd_diff <= rev_diff ? fwd_diff : -int32_t(rev_diff); - _encoderCounts[motor] += diff; - _lastEncoderCount[motor] = count; - - _motorSpeeds[motor] = speed; + if (receiveTransaction(CMD_READ_SPEED_2, buffer_speed_left, ENCODER_SPEED_MESSAGE_SIZE) < ENCODER_SPEED_MESSAGE_SIZE) { + return -1; } - return 1; -} + int32_t position_right = reverseInt32(&buffer_positon[0]); + int32_t position_left = reverseInt32(&buffer_positon[4]); + int32_t speed_right = reverseInt32(&buffer_speed_right[0]); + int32_t speed_left = reverseInt32(&buffer_speed_left[0]); -void RoboClaw::printStatus() -{ - PX4_ERR("pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n", - double(getMotorPosition(MOTOR_1)), - double(getMotorSpeed(MOTOR_1)), - double(getMotorPosition(MOTOR_2)), - double(getMotorSpeed(MOTOR_2))); -} - -float RoboClaw::getMotorPosition(e_motor motor) -{ - if (motor == MOTOR_1) { - return _encoderCounts[0]; - - } else if (motor == MOTOR_2) { - return _encoderCounts[1]; + wheel_encoders_s wheel_encoders{}; + wheel_encoders.wheel_angle[0] = static_cast(position_right) / _param_rbclw_counts_rev.get() * M_TWOPI_F; + wheel_encoders.wheel_angle[1] = static_cast(position_left) / _param_rbclw_counts_rev.get() * M_TWOPI_F; + wheel_encoders.wheel_speed[0] = static_cast(speed_right) / _param_rbclw_counts_rev.get() * M_TWOPI_F; + wheel_encoders.wheel_speed[1] = static_cast(speed_left) / _param_rbclw_counts_rev.get() * M_TWOPI_F; + wheel_encoders.timestamp = hrt_absolute_time(); + _wheel_encoders_pub.publish(wheel_encoders); - } else { - warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); - return NAN; - } + return 1; } -float RoboClaw::getMotorSpeed(e_motor motor) +int32_t RoboClaw::reverseInt32(uint8_t *buffer) { - if (motor == MOTOR_1) { - return _motorSpeeds[0]; - - } else if (motor == MOTOR_2) { - return _motorSpeeds[1]; - - } else { - warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); - return NAN; - } + return (buffer[0] << 24) + | (buffer[1] << 16) + | (buffer[2] << 8) + | buffer[3]; } -int RoboClaw::setMotorSpeed(e_motor motor, float value) +void RoboClaw::setMotorSpeed(e_motor motor, float value) { e_command command; @@ -410,15 +325,14 @@ int RoboClaw::setMotorSpeed(e_motor motor, float value) } } else { - return -1; + return; } - return _sendUnsigned7Bit(command, value); + _sendUnsigned7Bit(command, value); } -int RoboClaw::setMotorDutyCycle(e_motor motor, float value) +void RoboClaw::setMotorDutyCycle(e_motor motor, float value) { - e_command command; // send command @@ -429,56 +343,48 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value) command = CMD_SIGNED_DUTYCYCLE_2; } else { - return -1; + return; } return _sendSigned16Bit(command, value); } -int RoboClaw::drive(float value) +void RoboClaw::drive(float value) { e_command command = value >= 0 ? CMD_DRIVE_FWD_MIX : CMD_DRIVE_REV_MIX; - return _sendUnsigned7Bit(command, value); + _sendUnsigned7Bit(command, value); } -int RoboClaw::turn(float value) +void RoboClaw::turn(float value) { e_command command = value >= 0 ? CMD_TURN_LEFT : CMD_TURN_RIGHT; - return _sendUnsigned7Bit(command, value); + _sendUnsigned7Bit(command, value); } -int RoboClaw::resetEncoders() +void RoboClaw::resetEncoders() { - return _sendNothing(CMD_RESET_ENCODERS); + sendTransaction(CMD_RESET_ENCODERS, nullptr, 0); } -int RoboClaw::_sendUnsigned7Bit(e_command command, float data) +void RoboClaw::_sendUnsigned7Bit(e_command command, float data) { data = fabs(data); - if (data > 1.0f) { - data = 1.0f; + if (data >= 1.0f) { + data = 0.99f; } auto byte = (uint8_t)(data * INT8_MAX); - uint8_t recv_byte; - return _transaction(command, &byte, 1, &recv_byte, 1); + sendTransaction(command, &byte, 1); } -int RoboClaw::_sendSigned16Bit(e_command command, float data) +void RoboClaw::_sendSigned16Bit(e_command command, float data) { int16_t duty = math::constrain(data, -1.f, 1.f) * INT16_MAX; uint8_t buff[2]; buff[0] = (duty >> 8) & 0xFF; // High byte buff[1] = duty & 0xFF; // Low byte - uint8_t recv_buff; - return _transaction(command, (uint8_t *) &buff, 2, &recv_buff, 2); -} - -int RoboClaw::_sendNothing(e_command command) -{ - uint8_t recv_buff; - return _transaction(command, nullptr, 0, &recv_buff, 1); + sendTransaction(command, (uint8_t *) &buff, 2); } uint16_t RoboClaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init) @@ -501,190 +407,137 @@ uint16_t RoboClaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init) return crc; } -int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, uint8_t *rbuff, size_t rbytes, bool send_checksum, bool recv_checksum) +int RoboClaw::receiveTransaction(e_command cmd, uint8_t *read_buffer, size_t bytes_to_read) { + if (writeCommand(cmd) != RoboClawError::Success) { + return -1; + } + + size_t total_bytes_read = 0; + + while (total_bytes_read < bytes_to_read) { + int select_status = select(_uart_fd + 1, &_uart_fd_set, nullptr, nullptr, &_uart_fd_timeout); + + if (select_status <= 0) { + PX4_ERR("Select timeout %d\n", select_status); + return -1; + } - // Construct the packet to be sent. - uint8_t buf[wbytes + 4]; - buf[0] = (uint8_t) _parameters.address; - buf[1] = cmd; + int bytes_read = read(_uart_fd, &read_buffer[total_bytes_read], bytes_to_read - total_bytes_read); + if (bytes_read <= 0) { + PX4_ERR("Read timeout %d\n", select_status); + return -1; + } - RoboClawError err_code = writeToDevice(cmd, wbuff, wbytes, send_checksum, buf); + total_bytes_read += bytes_read; + } - if (err_code != RoboClawError::Success) { - printError(err_code); + if (total_bytes_read < 2) { + PX4_ERR("Too short payload received\n"); return -1; } - err_code = readFromDevice(rbuff, rbytes, recv_checksum, buf); + // Verify checksum + uint8_t address = static_cast(_param_rbclw_address.get()); + uint8_t command = static_cast(cmd); + uint16_t checksum_calc = _calcCRC(&address, 1); // address + checksum_calc = _calcCRC(&command, 1, checksum_calc); // command + checksum_calc = _calcCRC(read_buffer, total_bytes_read - 2, checksum_calc); // received payload + uint16_t checksum_recv = (read_buffer[total_bytes_read - 2] << 8) + read_buffer[total_bytes_read - 1]; - if (err_code != RoboClawError::Success) { - printError(err_code); + if (checksum_calc != checksum_recv) { + PX4_ERR("Checksum mismatch\n"); return -1; } - return 1; + return total_bytes_read; } -RoboClaw::RoboClawError RoboClaw::writeToDevice(e_command cmd, uint8_t *wbuff, size_t wbytes, bool send_checksum, uint8_t *buf) +void RoboClaw::sendTransaction(e_command cmd, uint8_t *write_buffer, size_t bytes_to_write) { + writeCommandWithPayload(cmd, write_buffer, bytes_to_write); + int select_status = select(_uart_fd + 1, &_uart_fd_set, nullptr, nullptr, &_uart_fd_timeout); - tcflush(_uart, TCIOFLUSH); // flush the buffers - - if (wbuff) { - memcpy(&buf[2], wbuff, wbytes); + if (select_status <= 0) { + PX4_ERR("ACK timeout"); + return; } - // Compute and append checksum if necessary. - wbytes = wbytes + (send_checksum ? 4 : 2); + uint8_t acknowledgement{0}; + int bytes_read = read(_uart_fd, &acknowledgement, 1); - if (send_checksum) { - uint16_t sum = _calcCRC(buf, wbytes - 2); - buf[wbytes - 2] = (sum >> 8) & 0xFF; - buf[wbytes - 1] = sum & 0xFFu; + if ((bytes_read != 1) || (acknowledgement != 0xFF)) { + PX4_ERR("ACK wrong"); + return; } +} + +RoboClaw::RoboClawError RoboClaw::writeCommand(e_command cmd) +{ + uint8_t buffer[2]; - // Write data to the device. + buffer[0] = (uint8_t)_param_rbclw_address.get(); + buffer[1] = cmd; - int count = write(_uart, buf, wbytes); + size_t count = write(_uart_fd, buffer, 2); - // Error checking for the write operation. - if (count < (int) wbytes) { // Did not successfully send all bytes. - PX4_ERR("Only wrote %d out of %zu bytes", count, wbytes); + if (count < 2) { + PX4_ERR("Only wrote %d out of %zu bytes", count, 2); return RoboClawError::WriteError; } - return RoboClawError::Success; // Return success if write operation is successful + return RoboClawError::Success; } -RoboClaw::RoboClawError RoboClaw::readFromDevice(uint8_t *rbuff, size_t rbytes, bool recv_checksum, uint8_t *buf) +RoboClaw::RoboClawError RoboClaw::writeCommandWithPayload(e_command cmd, uint8_t *wbuff, size_t bytes_to_write) { + size_t packet_size = 2 + bytes_to_write + 2; + uint8_t buffer[packet_size]; - uint8_t *rbuff_curr = rbuff; - size_t bytes_read = 0; - - while (bytes_read < rbytes) { - - _uart_timeout.tv_sec = 0; - _uart_timeout.tv_usec = TIMEOUT_US; - - int select_status = select(_uart + 1, &_uart_set, nullptr, nullptr, &_uart_timeout); - - if (select_status <= 0) { - // Handle select error or timeout here - PX4_ERR("error %f", (double)select_status); - - usleep(20000000); // 20 second delay - - PX4_ERR("Trying again to reconnect to RoboClaw"); - - // Reinitialize the roboclaw driver - initialize(); - - // return RoboClawError::ReadTimeout; - - } + // Add address + command ID + buffer[0] = (uint8_t) _param_rbclw_address.get(); + buffer[1] = cmd; - int err_code = read(_uart, rbuff_curr, rbytes - bytes_read); - - if (err_code <= 0) { - // Handle read error here - return RoboClawError::ReadError; - } - - bytes_read += err_code; - rbuff_curr += err_code; + // Add payload + if (bytes_to_write > 0 && wbuff) { + memcpy(&buffer[2], wbuff, bytes_to_write); } - if (recv_checksum) { + // Add checksum + uint16_t sum = _calcCRC(buffer, packet_size - 2); + buffer[packet_size - 2] = (sum >> 8) & 0xFF; + buffer[packet_size - 1] = sum & 0xFFu; - if (bytes_read < 2) { - return RoboClawError::ChecksumError; // or an equivalent error code of your choosing - } - - uint16_t checksum_calc = _calcCRC(buf, 2); // assuming buf contains address and command - checksum_calc = _calcCRC(rbuff, bytes_read - 2, checksum_calc); - uint16_t checksum_recv = (rbuff[bytes_read - 2] << 8) + rbuff[bytes_read - 1]; + // Write to device + size_t count = write(_uart_fd, buffer, packet_size); - if (checksum_calc != checksum_recv) { - return RoboClawError::ChecksumError; - } + // Not all bytes sent + if (count < packet_size) { + PX4_ERR("Only wrote %d out of %d bytes", count, bytes_to_write); + return RoboClawError::WriteError; } - - return RoboClawError::Success; // Return success if everything went well + return RoboClawError::Success; } -void RoboClaw::printError(RoboClawError error) +int RoboClaw::custom_command(int argc, char *argv[]) { - - switch (error) { - case RoboClawError::Success: - break; - case RoboClawError::WriteError: - PX4_ERR("Failed to write all bytes to the device"); - break; - case RoboClawError::ReadError: - PX4_ERR("Failed to read from the device"); - break; - case RoboClawError::ReadTimeout: - PX4_ERR("Timeout while reading from the device"); - break; - default: - PX4_ERR("Unknown error code"); - } + return 0; } - -void RoboClaw::_parameters_update() +int RoboClaw::print_usage(const char *reason) { - 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); - - if (_actuatorsSub > 0) { - orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms); - } - - int32_t baudRate; - param_get(_param_handles.serial_baud_rate, &baudRate); - - switch (baudRate) { - case 2400: - _parameters.serial_baud_rate = B2400; - break; - - case 9600: - _parameters.serial_baud_rate = B9600; - break; - - case 19200: - _parameters.serial_baud_rate = B19200; - break; - - case 38400: - _parameters.serial_baud_rate = B38400; - break; - - case 57600: - _parameters.serial_baud_rate = B57600; - break; - - case 115200: - _parameters.serial_baud_rate = B115200; - break; - - case 230400: - _parameters.serial_baud_rate = B230400; - break; + return 0; +} - case 460800: - _parameters.serial_baud_rate = B460800; - break; +int RoboClaw::print_status() +{ + PX4_ERR("Running, Initialized: %f", (double)_initialized); + return 0; +} - default: - _parameters.serial_baud_rate = B2400; - } +extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]) +{ + return RoboClaw::main(argc, argv); } diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index abbb33985770..681594eb2415 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2023 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 @@ -53,25 +53,45 @@ #include #include +#include + #include -#include #include #include #include -#include -#include -#include +#include + +#include + +#include +#include +#include +#include +#include + +#include /** * This is a driver for the RoboClaw motor controller */ -class RoboClaw +class RoboClaw : public ModuleBase, public OutputModuleInterface { public: + /** + * constructor + * @param deviceName the name of the + * serial port e.g. "/dev/ttyS2" + * @param address the adddress of the motor + * (selectable on roboclaw) + * @param baudRateParam Name of the parameter that holds the baud rate of this serial port + */ + RoboClaw(const char *deviceName, const char *baudRateParam); - void taskMain(); - static bool taskShouldExit; + /** + * deconstructor + */ + virtual ~RoboClaw(); /** control channels */ enum e_channel { @@ -96,77 +116,68 @@ class RoboClaw ReadTimeout }; - /** - * constructor - * @param deviceName the name of the - * serial port e.g. "/dev/ttyS2" - * @param address the adddress of the motor - * (selectable on roboclaw) - * @param baudRateParam Name of the parameter that holds the baud rate of this serial port - */ - RoboClaw(const char *deviceName, const char *baudRateParam); + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); - /** - * deconstructor - */ - virtual ~RoboClaw(); + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); - void initialize(); + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); - /** - * @return position of a motor, rev - */ - float getMotorPosition(e_motor motor); + /** @see ModuleBase::print_status() */ + int print_status() override; - /** - * @return speed of a motor, rev/sec - */ - float getMotorSpeed(e_motor motor); + void Run() override; + + int init(); + + bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], + unsigned num_outputs, unsigned num_control_groups_updated) override; /** * set the speed of a motor, rev/sec */ - int setMotorSpeed(e_motor motor, float value); + void setMotorSpeed(e_motor motor, float value); /** * set the duty cycle of a motor */ - int setMotorDutyCycle(e_motor motor, float value); + void setMotorDutyCycle(e_motor motor, float value); /** * Drive both motors. +1 = full forward, -1 = full backward */ - int drive(float value); + void drive(float value); /** * Turn. +1 = full right, -1 = full left */ - int turn(float value); + void turn(float value); /** * reset the encoders * @return status */ - int resetEncoders(); + void resetEncoders(); /** * read data from serial */ int readEncoder(); - /** - * print status - */ - void printStatus(); - - private: + static constexpr int MAX_ACTUATORS = 2; + + MixingOutput _mixing_output{"ROBOCLAW", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false}; char _storedDeviceName[256]; // Adjust size as necessary - char _storedBaudRateParam[256]; // Adjust size as necessary + char _storedBaudRateParam[256]; // Adjust size as necessary int _timeout_counter = 0; + bool _successfully_connected{false}; + // commands // We just list the commands we want from the manual here. enum e_command { @@ -200,99 +211,39 @@ class RoboClaw CMD_READ_STATUS = 90 }; + int _uart_fd{0}; + fd_set _uart_fd_set; + struct timeval _uart_fd_timeout; - struct { - speed_t serial_baud_rate; - int32_t counts_per_rev; - 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 encoder_read_period_ms; - param_t actuator_write_period_ms; - param_t address; - } _param_handles{}; - - int _uart; - fd_set _uart_set; - struct timeval _uart_timeout; + uORB::SubscriptionData _actuator_armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription _differential_drive_control_sub{ORB_ID(differential_drive_control)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - /** actuator controls subscription */ - int _actuatorsSub{-1}; - actuator_controls_s _actuatorControls; + differential_drive_control_s _diff_drive_control{}; - int _armedSub{-1}; - actuator_armed_s _actuatorArmed; + matrix::Vector2f _motor_control{0.0f, 0.0f}; - int _paramSub{-1}; - parameter_update_s _paramUpdate; - - uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)}; - uORB::Subscription _vehicle_torque_setpoint_sub{ORB_ID(vehicle_torque_setpoint)}; - - vehicle_thrust_setpoint_s vehicle_thrust_setpoint{}; - vehicle_torque_setpoint_s vehicle_torque_setpoint{}; - - - - uORB::PublicationMulti _wheelEncodersAdv[2] { ORB_ID(wheel_encoders), ORB_ID(wheel_encoders)}; - wheel_encoders_s _wheelEncoderMsg[2]; - - uint32_t _lastEncoderCount[2] {0, 0}; - int64_t _encoderCounts[2] {0, 0}; - int32_t _motorSpeeds[2] {0, 0}; + uORB::Publication _wheel_encoders_pub {ORB_ID(wheel_encoders)}; void _parameters_update(); - void vehicle_control_poll(); 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); - int _sendNothing(e_command); + void _sendUnsigned7Bit(e_command command, float data); + void _sendSigned16Bit(e_command command, float data); + int32_t reverseInt32(uint8_t *buffer); - /** - * print status - */ - RoboClawError writeToDevice(e_command cmd, uint8_t *wbuff, size_t wbytes, bool send_checksum, uint8_t *buf); + bool _initialized{false}; - /** - * print status - */ - RoboClawError readFromDevice(uint8_t *rbuff, size_t rbytes, bool recv_checksum, uint8_t *buf); + RoboClawError writeCommand(e_command cmd); + RoboClawError writeCommandWithPayload(e_command cmd, uint8_t *wbuff, size_t bytes_to_write); - /** - * print status - */ - void printError(RoboClawError err_code); + void sendTransaction(e_command cmd, uint8_t *write_buffer, size_t bytes_to_write); + int receiveTransaction(e_command cmd, uint8_t *read_buffer, size_t bytes_to_read); - /** - * Perform a round-trip write and read. - * - * NOTE: This function is not thread-safe. - * - * @param cmd Command to send to the Roboclaw - * @param wbuff Write buffer. Must not contain command, address, or checksum. For most commands, this will be - * one or two bytes. Can be null iff wbytes == 0. - * @param wbytes Number of bytes to write. Can be 0. - * @param rbuff Read buffer. Will be filled with the entire response, including a checksum if the Roboclaw sends - * a checksum for this command. - * @param rbytes Maximum number of bytes to read. - * @param send_checksum If true, then the checksum will be calculated and sent to the Roboclaw. - * This is an option because some Roboclaw commands expect no checksum. - * @param recv_checksum If true, then this function will calculate the checksum of the returned data and compare - * it to the checksum received. If they are not equal, OR if fewer than 2 bytes were received, then an - * error is returned. - * If false, then this function will expect to read exactly one byte, 0xFF, and will return an error otherwise. - * @return If successful, then the number of bytes read from the Roboclaw is returned. If there is a timeout - * reading from the Roboclaw, then 0 is returned. If there is an IO error, then a negative value is returned. - */ - int _transaction(e_command cmd, uint8_t *wbuff, size_t wbytes, - uint8_t *rbuff, size_t rbytes, bool send_checksum = true, bool recv_checksum = false); - + DEFINE_PARAMETERS( + (ParamInt) _param_rbclw_address, + (ParamInt) _param_rbclw_counts_rev + ) }; From 4edc4ed83b8d1dc6c4964c8adec08123dd68d934 Mon Sep 17 00:00:00 2001 From: Per Frivik <94360401+PerFrivik@users.noreply.github.com> Date: Fri, 10 Nov 2023 10:47:42 +0100 Subject: [PATCH 11/27] Update msg/CMakeLists.txt Remove ActuatorControls.msg Co-authored-by: Daniel Agar --- msg/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index ea3dff55ec30..25bbd4861b7e 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -169,7 +169,6 @@ set(msg_files RegisterExtComponentReply.msg RegisterExtComponentRequest.msg Rpm.msg - ActuatorControls.msg RtlTimeEstimate.msg SatelliteInfo.msg SensorAccel.msg From 5e9ed2dbc96569d70dce7fd46df7e71068a4e305 Mon Sep 17 00:00:00 2001 From: Per Frivik <94360401+PerFrivik@users.noreply.github.com> Date: Fri, 10 Nov 2023 11:05:33 +0100 Subject: [PATCH 12/27] Delete msg/ActuatorControls.msg --- msg/ActuatorControls.msg | 28 ---------------------------- 1 file changed, 28 deletions(-) delete mode 100644 msg/ActuatorControls.msg diff --git a/msg/ActuatorControls.msg b/msg/ActuatorControls.msg deleted file mode 100644 index c4b52ea48dfe..000000000000 --- a/msg/ActuatorControls.msg +++ /dev/null @@ -1,28 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -uint8 NUM_ACTUATOR_CONTROLS = 8 -uint8 NUM_ACTUATOR_CONTROL_GROUPS = 6 -uint8 INDEX_ROLL = 0 -uint8 INDEX_PITCH = 1 -uint8 INDEX_YAW = 2 -uint8 INDEX_THROTTLE = 3 -uint8 INDEX_FLAPS = 4 -uint8 INDEX_SPOILERS = 5 -uint8 INDEX_AIRBRAKES = 6 -uint8 INDEX_LANDING_GEAR = 7 -uint8 INDEX_GIMBAL_SHUTTER = 3 -uint8 INDEX_CAMERA_ZOOM = 4 - -uint8 GROUP_INDEX_ATTITUDE = 0 -uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1 -uint8 GROUP_INDEX_GIMBAL = 2 -uint8 GROUP_INDEX_MANUAL_PASSTHROUGH = 3 -uint8 GROUP_INDEX_ALLOCATED_PART1 = 4 -uint8 GROUP_INDEX_ALLOCATED_PART2 = 5 -uint8 GROUP_INDEX_PAYLOAD = 6 - -uint64 timestamp_sample # the timestamp the data this control response is based on was sampled -float32[8] control - -# TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3 -# TOPICS actuator_controls_4 actuator_controls_5 -# TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc From d9daaa93e9c38e85d107b21ca84b807cdf99e3fd Mon Sep 17 00:00:00 2001 From: Per Frivik <94360401+PerFrivik@users.noreply.github.com> Date: Fri, 10 Nov 2023 11:24:22 +0100 Subject: [PATCH 13/27] Update WheelEncoders.msg --- msg/WheelEncoders.msg | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/msg/WheelEncoders.msg b/msg/WheelEncoders.msg index 1654902cdf00..dfeeeca4322c 100644 --- a/msg/WheelEncoders.msg +++ b/msg/WheelEncoders.msg @@ -1,5 +1,5 @@ uint64 timestamp # time since system start (microseconds) -int64 encoder_position # The wheel position, in encoder counts since boot. Positive is forward rotation, negative is reverse rotation -int32 speed # Speed of each wheel, in encoder counts per second. Positive is forward, negative is reverse -uint32 pulses_per_rev # Number of pulses per revolution for each wheel +float32[4] wheel_angle #Wheel angle of encoder, if two wheels wheel_angle[0] = right wheel and wheel_angle[1] = left wheel. Else [0][1] = right and [2][3] are left. +float32[4] wheel_speed #Wheel speed of encoder, if two wheels wheel_angle[0] = right wheel and wheel_angle[1] = left wheel. Else [0][1] = right and [2][3] are left. + From 11369992421cb3baa524008ee50833084748c4ea Mon Sep 17 00:00:00 2001 From: Per Frivik <94360401+PerFrivik@users.noreply.github.com> Date: Fri, 10 Nov 2023 11:38:29 +0100 Subject: [PATCH 14/27] Update RoverPositionControl.cpp --- src/modules/rover_pos_control/RoverPositionControl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 1c80808ec011..24909280e99c 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -113,7 +113,7 @@ RoverPositionControl::vehicle_control_mode_poll() void RoverPositionControl::manual_control_setpoint_poll() { - if (true) { + if (_control_mode.flag_control_manual_enabled) { if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) { float dt = math::constrain(hrt_elapsed_time(&_manual_setpoint_last_called) * 1e-6f, 0.0002f, 0.04f); @@ -431,7 +431,7 @@ RoverPositionControl::Run() matrix::Vector2d current_position(_global_pos.lat, _global_pos.lon); matrix::Vector3f current_velocity(_local_pos.vx, _local_pos.vy, _local_pos.vz); - if (false) { + if (!_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) { if (control_position(current_position, ground_speed, _pos_sp_triplet)) { From b56fc3a6a7c52194e1a052e9f8fb78c3afbc8046 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 13 Nov 2023 16:37:56 +0100 Subject: [PATCH 15/27] RoboClaw: fix style --- src/drivers/roboclaw/RoboClaw.cpp | 21 ++++++++++++++------- src/drivers/roboclaw/RoboClaw.hpp | 2 +- 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 341d94655406..35533d8ee7f1 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -94,7 +94,8 @@ RoboClaw::~RoboClaw() close(_uart_fd); } -int RoboClaw::init() { +int RoboClaw::init() +{ _uart_fd_timeout = { .tv_sec = 0, .tv_usec = TIMEOUT_US }; int32_t baud_rate_parameter_value{0}; @@ -156,12 +157,15 @@ int RoboClaw::init() { // Set baud rate ret = cfsetispeed(&uart_config, baud_rate_posix); + if (ret < 0) { err(1, "failed to set input speed"); } ret = cfsetospeed(&uart_config, baud_rate_posix); + if (ret < 0) { err(1, "failed to set output speed"); } ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); + if (ret < 0) { err(1, "failed to set attr"); } FD_ZERO(&_uart_fd_set); @@ -174,6 +178,7 @@ int RoboClaw::init() { if (err_code <= 0) { PX4_ERR("Shutting down Roboclaw driver."); return PX4_ERROR; + } else { PX4_INFO("Successfully connected"); /* Schedule a cycle to start things. */ @@ -210,19 +215,21 @@ int RoboClaw::task_spawn(int argc, char *argv[]) } bool RoboClaw::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) + unsigned num_outputs, unsigned num_control_groups_updated) { - float left_motor_output = -((float)outputs[1] - 128.0f)/127.f; - float right_motor_output = ((float)outputs[0] - 128.0f)/127.f; + float left_motor_output = -((float)outputs[1] - 128.0f) / 127.f; + float right_motor_output = ((float)outputs[0] - 128.0f) / 127.f; - if(stop_motors){ + if (stop_motors) { setMotorSpeed(MOTOR_1, 0.f); setMotorSpeed(MOTOR_2, 0.f); + } else { // PX4_ERR("Left and Right motor speed %f %f", (double)left_motor_output, (double)right_motor_output); setMotorSpeed(MOTOR_1, left_motor_output); setMotorSpeed(MOTOR_2, right_motor_output); } + return true; } @@ -237,12 +244,12 @@ void RoboClaw::Run() _mixing_output.update(); - if(!_initialized){ + if (!_initialized) { init(); _initialized = true; } - // check for parameter updates + // check for parameter updates if (_parameter_update_sub.updated()) { // clear update parameter_update_s pupdate; diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index 681594eb2415..381deb08aaec 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -133,7 +133,7 @@ class RoboClaw : public ModuleBase, public OutputModuleInterface int init(); bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], - unsigned num_outputs, unsigned num_control_groups_updated) override; + unsigned num_outputs, unsigned num_control_groups_updated) override; /** * set the speed of a motor, rev/sec From 18b15903c2d0c20abed061bc5b2c9bc723236bfa Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 13 Nov 2023 17:51:58 +0100 Subject: [PATCH 16/27] RoboClaw: declutter, make it compile again --- boards/px4/fmu-v5x/default.px4board | 2 +- msg/CMakeLists.txt | 2 +- msg/WheelEncoders.msg | 1 - src/drivers/roboclaw/CMakeLists.txt | 9 +- src/drivers/roboclaw/Kconfig | 2 +- src/drivers/roboclaw/RoboClaw.cpp | 34 ++- src/drivers/roboclaw/RoboClaw.hpp | 35 +-- src/drivers/roboclaw/RoboclawSerialDevice.hpp | 66 ------ src/drivers/roboclaw/roboclaw_main.cpp | 200 ------------------ src/drivers/roboclaw/roboclaw_params.c | 55 +---- 10 files changed, 40 insertions(+), 366 deletions(-) delete mode 100644 src/drivers/roboclaw/RoboclawSerialDevice.hpp delete mode 100644 src/drivers/roboclaw/roboclaw_main.cpp diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index 80e45a0074c8..c9300dba0703 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -15,7 +15,7 @@ CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y -CONFIG_COMMON_DISTANCE_SENSOR=n +CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_ROBOCLAW=y CONFIG_DRIVERS_GPIO_MCP23009=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 25bbd4861b7e..8327c7b3bd3f 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -234,8 +234,8 @@ set(msg_files VehicleTrajectoryBezier.msg VehicleTrajectoryWaypoint.msg VtolVehicleStatus.msg - Wind.msg WheelEncoders.msg + Wind.msg YawEstimatorStatus.msg ) list(SORT msg_files) diff --git a/msg/WheelEncoders.msg b/msg/WheelEncoders.msg index dfeeeca4322c..368aa9360bad 100644 --- a/msg/WheelEncoders.msg +++ b/msg/WheelEncoders.msg @@ -2,4 +2,3 @@ uint64 timestamp # time since system start (microseconds) float32[4] wheel_angle #Wheel angle of encoder, if two wheels wheel_angle[0] = right wheel and wheel_angle[1] = left wheel. Else [0][1] = right and [2][3] are left. float32[4] wheel_speed #Wheel speed of encoder, if two wheels wheel_angle[0] = right wheel and wheel_angle[1] = left wheel. Else [0][1] = right and [2][3] are left. - diff --git a/src/drivers/roboclaw/CMakeLists.txt b/src/drivers/roboclaw/CMakeLists.txt index daad2f40b97c..ee2570a68742 100644 --- a/src/drivers/roboclaw/CMakeLists.txt +++ b/src/drivers/roboclaw/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-2023 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 @@ -31,18 +31,11 @@ # ############################################################################ -set(PARAM_PREFIX ROBOCLAW) - -if(CONFIG_BOARD_IO) - set(PARAM_PREFIX ROBOCLAW) -endif() - px4_add_module( MODULE drivers__roboclaw MAIN roboclaw COMPILE_FLAGS SRCS - roboclaw_main.cpp RoboClaw.cpp MODULE_CONFIG module.yaml diff --git a/src/drivers/roboclaw/Kconfig b/src/drivers/roboclaw/Kconfig index 696c14023346..93395269a8d1 100644 --- a/src/drivers/roboclaw/Kconfig +++ b/src/drivers/roboclaw/Kconfig @@ -1,5 +1,5 @@ menuconfig DRIVERS_ROBOCLAW - bool "crsf_rc" + bool "roboclaw" default n ---help--- Enable support for roboclaw diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 35533d8ee7f1..9dafe6859218 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -78,9 +78,7 @@ using namespace time_literals; RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam) : - // ModuleParams(nullptr), OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default) - // ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default) { strncpy(_storedDeviceName, deviceName, sizeof(_storedDeviceName) - 1); _storedDeviceName[sizeof(_storedDeviceName) - 1] = '\0'; // Ensure null-termination @@ -251,9 +249,9 @@ void RoboClaw::Run() // check for parameter updates if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); + // Read from topic to clear updated flag + parameter_update_s parameter_update; + _parameter_update_sub.copy(¶meter_update); updateParams(); } @@ -530,11 +528,35 @@ RoboClaw::RoboClawError RoboClaw::writeCommandWithPayload(e_command cmd, uint8_t int RoboClaw::custom_command(int argc, char *argv[]) { - return 0; + return print_usage("unknown command"); } int RoboClaw::print_usage(const char *reason) { + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION(R"DESCR_STR( +### Description + +This driver communicates over UART with the [Roboclaw motor driver](https://www.basicmicro.com/motor-controller). +It performs two tasks: + + - Control the motors based on the OutputModuleInterface. + - Read the wheel encoders and publish the raw data in the `wheel_encoders` uORB topic + +In order to use this driver, the Roboclaw should be put into Packet Serial mode (see the linked documentation), and +your flight controller's UART port should be connected to the Roboclaw as shown in the documentation. +The driver needs to be enabled using the parameter `RBCLW_SER_CFG`, the baudrate needs to be set correctly and +the address `RBCLW_ADDRESS` needs to match the ESC configuration. + +The command to start this driver is: `$ roboclaw start ` +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("roboclaw", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index 381deb08aaec..008332ef1390 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -36,41 +36,23 @@ * * RoboClaw Motor Driver * - * references: - * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf - * + * Product page: https://www.basicmicro.com/motor-controller + * Manual: https://downloads.basicmicro.com/docs/roboclaw_user_manual.pdf */ #pragma once -#include -#include -#include -#include -#include +#include +#include +#include #include -#include -#include -#include #include - -#include -#include #include #include -#include - -#include - -#include -#include -#include -#include -#include - -#include +#include +#include /** * This is a driver for the RoboClaw motor controller @@ -216,11 +198,8 @@ class RoboClaw : public ModuleBase, public OutputModuleInterface struct timeval _uart_fd_timeout; uORB::SubscriptionData _actuator_armed_sub{ORB_ID(actuator_armed)}; - uORB::Subscription _differential_drive_control_sub{ORB_ID(differential_drive_control)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - differential_drive_control_s _diff_drive_control{}; - matrix::Vector2f _motor_control{0.0f, 0.0f}; uORB::Publication _wheel_encoders_pub {ORB_ID(wheel_encoders)}; diff --git a/src/drivers/roboclaw/RoboclawSerialDevice.hpp b/src/drivers/roboclaw/RoboclawSerialDevice.hpp deleted file mode 100644 index f70f3c5385b3..000000000000 --- a/src/drivers/roboclaw/RoboclawSerialDevice.hpp +++ /dev/null @@ -1,66 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2021 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 RoboclawSerialDevice.hpp - * @brief - * @author Matthias Grob - */ - -#include "RoboclawDriver/RoboclawDriver.hpp" -#include - -class RoboclawDevice : public RoboclawWritableInterface -{ -public: - RoboclawDevice(const char *port); - ~RoboclawDevice(); - int init(); - void printStatus(); - -private: - void Run(); - size_t writeCallback(const uint8_t *buffer, const uint16_t length) override; - int setBaudrate(const unsigned baudrate); - - static constexpr size_t READ_BUFFER_SIZE{256}; - static constexpr int DISARM_VALUE = 0; - static constexpr int MIN_THROTTLE = 100; - static constexpr int MAX_THROTTLE = 1000; - - char _port[20] {}; - int _serial_fd{-1}; - VescDriver _vesc_driver; - MixingOutput _mixing_output{4, *this, MixingOutput::SchedulingPolicy::Auto, false, false}; - perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; -}; diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp deleted file mode 100644 index a3483c25a548..000000000000 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ /dev/null @@ -1,200 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 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_main.cpp - * - * RoboClaw Motor Driver - * - * references: - * http://downloads.ionmc.com/docs/roboclaw_user_manual.pdf - * - */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - - -#include "RoboClaw.hpp" - -static bool thread_running = false; /**< Deamon status flag */ -px4_task_t deamon_task; - -/** - * Deamon management function. - */ -extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int roboclaw_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(); - -static void usage() -{ - PRINT_MODULE_USAGE_NAME("roboclaw", "driver"); - - PRINT_MODULE_DESCRIPTION(R"DESCR_STR( -### Description - -This driver communicates over UART with the [Roboclaw motor driver](https://www.basicmicro.com/motor-controller). -It performs two tasks: - - - Control the motors based on the `actuator_controls_0` uORB topic. - - Read the wheel encoders and publish the raw data in the `wheel_encoders` uORB topic - -In order to use this driver, the Roboclaw should be put into Packet Serial mode (see the linked documentation), and -your flight controller's UART port should be connected to the Roboclaw as shown in the documentation. For Pixhawk 4, -use the `UART & I2C B` port, which corresponds to `/dev/ttyS3`. - -### Implementation - -The main loop of this module (Located in `RoboClaw.cpp::task_main()`) performs 2 tasks: - - 1. Write `actuator_controls_0` messages to the Roboclaw as they become available - 2. Read encoder data from the Roboclaw at a constant, fixed rate. - -Because of the latency of UART, this driver does not write every single `actuator_controls_0` message to the Roboclaw -immediately. Instead, it is rate limited based on the parameter `RBCLW_WRITE_PER`. - -On startup, this driver will attempt to read the status of the Roboclaw to verify that it is connected. If this fails, -the driver terminates immediately. - -### Examples - -The command to start this driver is: - - $ roboclaw start - -`` is the name of the UART port. On the Pixhawk 4, this is `/dev/ttyS3`. -`` is te baud rate. - -All available commands are: - - - `$ roboclaw start ` - - `$ roboclaw status` - - `$ roboclaw stop` - )DESCR_STR"); -} - -/** - * The deamon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int roboclaw_main(int argc, char *argv[]) -{ - if (!strcmp(argv[1], "start") && (argc >= 4)) { - - if (thread_running) { - printf("roboclaw already running\n"); - /* this is not an error */ - return 0; - } - - RoboClaw::taskShouldExit = false; - deamon_task = px4_task_spawn_cmd("roboclaw", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 10, - 2000, - roboclaw_thread_main, - (char *const *)argv); - return 0; - - } else if (!strcmp(argv[1], "stop")) { - - RoboClaw::taskShouldExit = true; - return 0; - - } else if (!strcmp(argv[1], "status")) { - - if (thread_running) { - printf("\troboclaw app is running\n"); - - } else { - printf("\troboclaw app not started\n"); - } - - return 0; - } - - usage(); - return 1; -} - -int roboclaw_thread_main(int argc, char *argv[]) -{ - printf("[roboclaw] starting\n"); - - // skip parent process args - argc -= 2; - argv += 2; - - if (argc < 2) { - printf("usage: roboclaw start \n"); - return -1; - } - - const char *deviceName = argv[1]; - const char *baudRate = argv[2]; - - // start - RoboClaw roboclaw(deviceName, baudRate); - - thread_running = true; - - roboclaw.taskMain(); - - // exit - printf("[roboclaw] exiting.\n"); - thread_running = false; - return 0; -} diff --git a/src/drivers/roboclaw/roboclaw_params.c b/src/drivers/roboclaw/roboclaw_params.c index 849d95663812..938e8530359a 100644 --- a/src/drivers/roboclaw/roboclaw_params.c +++ b/src/drivers/roboclaw/roboclaw_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2023 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 @@ -31,40 +31,6 @@ * ****************************************************************************/ -/** - * @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 * @@ -93,22 +59,3 @@ PARAM_DEFINE_INT32(RBCLW_COUNTS_REV, 1200); * @group Roboclaw driver */ PARAM_DEFINE_INT32(RBCLW_ADDRESS, 128); - -/** - * Roboclaw serial baud rate - * - * Baud rate of the serial communication with the Roboclaw. The Roboclaw must be configured to match this rate. - * @min 2400 - * @max 460800 - * @value 2400 2400 baud - * @value 9600 9600 baud - * @value 19200 19200 baud - * @value 38400 38400 baud - * @value 57600 57600 baud - * @value 115200 115200 baud - * @value 230400 230400 baud - * @value 460800 460800 baud - * @group Roboclaw driver - * @reboot_required true - */ -PARAM_DEFINE_INT32(RBCLW_BAUD, 57600); From b6999c2b0189d3cb6771ffa7ab81ec3bbbd062ef Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 14 Nov 2023 12:42:10 +0100 Subject: [PATCH 17/27] Roboclaw: move parameters to module.yaml --- src/drivers/roboclaw/module.yaml | 28 ++++++++++++ src/drivers/roboclaw/roboclaw_params.c | 61 -------------------------- 2 files changed, 28 insertions(+), 61 deletions(-) delete mode 100644 src/drivers/roboclaw/roboclaw_params.c diff --git a/src/drivers/roboclaw/module.yaml b/src/drivers/roboclaw/module.yaml index b501b1ecb380..e9dc4e9ddbb3 100644 --- a/src/drivers/roboclaw/module.yaml +++ b/src/drivers/roboclaw/module.yaml @@ -4,3 +4,31 @@ serial_config: port_config_param: name: RBCLW_SER_CFG group: Roboclaw + +parameters: + - group: Roboclaw Driver + definitions: + RBCLW_COUNTS_REV: + description: + short: Number of encoder counts for one wheel revolution + long: The default value of 1200 corresponds to the default configuration of the Aion R1 rover. + type: int32 + default: 1200 + min: 1 + RBCLW_ADDRESS: + description: + short: Address of the ESC on the bus + long: The ESC has to be configured to have an address from 0x80 to 0x87. This parameter needs to match the configured value. + type: enum + default: 128 + min: 128 + max: 135 + values: + 128: 0x80 + 129: 0x81 + 130: 0x82 + 131: 0x83 + 132: 0x84 + 133: 0x85 + 134: 0x86 + 135: 0x87 diff --git a/src/drivers/roboclaw/roboclaw_params.c b/src/drivers/roboclaw/roboclaw_params.c deleted file mode 100644 index 938e8530359a..000000000000 --- a/src/drivers/roboclaw/roboclaw_params.c +++ /dev/null @@ -1,61 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2023 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. - * - ****************************************************************************/ - -/** - * 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); - -/** - * 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); From 627244e3612d67555d008684d342a2e9529e3d13 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 14 Nov 2023 12:50:27 +0100 Subject: [PATCH 18/27] Rename RoboClaw -> Roboclaw The manufacturer uses both naming schemes, RoboClaw more than Roboclaw but it's always one word and hence I think it's more consistent to name it the latter. --- src/drivers/roboclaw/CMakeLists.txt | 2 +- .../roboclaw/{RoboClaw.cpp => Roboclaw.cpp} | 58 +++++++++---------- .../roboclaw/{RoboClaw.hpp => Roboclaw.hpp} | 10 ++-- 3 files changed, 35 insertions(+), 35 deletions(-) rename src/drivers/roboclaw/{RoboClaw.cpp => Roboclaw.cpp} (91%) rename src/drivers/roboclaw/{RoboClaw.hpp => Roboclaw.hpp} (96%) diff --git a/src/drivers/roboclaw/CMakeLists.txt b/src/drivers/roboclaw/CMakeLists.txt index ee2570a68742..4218a36553a2 100644 --- a/src/drivers/roboclaw/CMakeLists.txt +++ b/src/drivers/roboclaw/CMakeLists.txt @@ -36,7 +36,7 @@ px4_add_module( MAIN roboclaw COMPILE_FLAGS SRCS - RoboClaw.cpp + Roboclaw.cpp MODULE_CONFIG module.yaml ) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/Roboclaw.cpp similarity index 91% rename from src/drivers/roboclaw/RoboClaw.cpp rename to src/drivers/roboclaw/Roboclaw.cpp index 9dafe6859218..23333a0042ef 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/Roboclaw.cpp @@ -32,9 +32,9 @@ ****************************************************************************/ /** - * @file RoboClaw.cpp + * @file Roboclaw.cpp * - * RoboClaw Motor Driver + * Roboclaw Motor Driver * * references: * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf @@ -42,7 +42,7 @@ */ -#include "RoboClaw.hpp" +#include "Roboclaw.hpp" #include #include #include @@ -58,7 +58,7 @@ #include #include -// The RoboClaw has a serial communication timeout of 10ms. +// The Roboclaw has a serial communication timeout of 10ms. // Add a little extra to account for timing inaccuracy #define TIMEOUT_US 10500 @@ -77,7 +77,7 @@ using namespace time_literals; -RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam) : +Roboclaw::Roboclaw(const char *deviceName, const char *baudRateParam) : OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default) { strncpy(_storedDeviceName, deviceName, sizeof(_storedDeviceName) - 1); @@ -87,12 +87,12 @@ RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam) : _storedBaudRateParam[sizeof(_storedBaudRateParam) - 1] = '\0'; // Ensure null-termination } -RoboClaw::~RoboClaw() +Roboclaw::~Roboclaw() { close(_uart_fd); } -int RoboClaw::init() +int Roboclaw::init() { _uart_fd_timeout = { .tv_sec = 0, .tv_usec = TIMEOUT_US }; @@ -185,12 +185,12 @@ int RoboClaw::init() } } -int RoboClaw::task_spawn(int argc, char *argv[]) +int Roboclaw::task_spawn(int argc, char *argv[]) { const char *deviceName = argv[1]; const char *baud_rate_parameter_value = argv[2]; - RoboClaw *instance = new RoboClaw(deviceName, baud_rate_parameter_value); + Roboclaw *instance = new Roboclaw(deviceName, baud_rate_parameter_value); if (instance) { _object.store(instance); @@ -212,7 +212,7 @@ int RoboClaw::task_spawn(int argc, char *argv[]) return PX4_ERROR; } -bool RoboClaw::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], +bool Roboclaw::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { float left_motor_output = -((float)outputs[1] - 128.0f) / 127.f; @@ -231,7 +231,7 @@ bool RoboClaw::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], return true; } -void RoboClaw::Run() +void Roboclaw::Run() { if (should_exit()) { ScheduleClear(); @@ -266,7 +266,7 @@ void RoboClaw::Run() } -int RoboClaw::readEncoder() +int Roboclaw::readEncoder() { uint8_t buffer_positon[ENCODER_MESSAGE_SIZE]; uint8_t buffer_speed_right[ENCODER_SPEED_MESSAGE_SIZE]; @@ -300,7 +300,7 @@ int RoboClaw::readEncoder() return 1; } -int32_t RoboClaw::reverseInt32(uint8_t *buffer) +int32_t Roboclaw::reverseInt32(uint8_t *buffer) { return (buffer[0] << 24) | (buffer[1] << 16) @@ -308,7 +308,7 @@ int32_t RoboClaw::reverseInt32(uint8_t *buffer) | buffer[3]; } -void RoboClaw::setMotorSpeed(e_motor motor, float value) +void Roboclaw::setMotorSpeed(e_motor motor, float value) { e_command command; @@ -336,7 +336,7 @@ void RoboClaw::setMotorSpeed(e_motor motor, float value) _sendUnsigned7Bit(command, value); } -void RoboClaw::setMotorDutyCycle(e_motor motor, float value) +void Roboclaw::setMotorDutyCycle(e_motor motor, float value) { e_command command; @@ -354,24 +354,24 @@ void RoboClaw::setMotorDutyCycle(e_motor motor, float value) return _sendSigned16Bit(command, value); } -void RoboClaw::drive(float value) +void Roboclaw::drive(float value) { e_command command = value >= 0 ? CMD_DRIVE_FWD_MIX : CMD_DRIVE_REV_MIX; _sendUnsigned7Bit(command, value); } -void RoboClaw::turn(float value) +void Roboclaw::turn(float value) { e_command command = value >= 0 ? CMD_TURN_LEFT : CMD_TURN_RIGHT; _sendUnsigned7Bit(command, value); } -void RoboClaw::resetEncoders() +void Roboclaw::resetEncoders() { sendTransaction(CMD_RESET_ENCODERS, nullptr, 0); } -void RoboClaw::_sendUnsigned7Bit(e_command command, float data) +void Roboclaw::_sendUnsigned7Bit(e_command command, float data) { data = fabs(data); @@ -383,7 +383,7 @@ void RoboClaw::_sendUnsigned7Bit(e_command command, float data) sendTransaction(command, &byte, 1); } -void RoboClaw::_sendSigned16Bit(e_command command, float data) +void Roboclaw::_sendSigned16Bit(e_command command, float data) { int16_t duty = math::constrain(data, -1.f, 1.f) * INT16_MAX; uint8_t buff[2]; @@ -392,7 +392,7 @@ void RoboClaw::_sendSigned16Bit(e_command command, float data) sendTransaction(command, (uint8_t *) &buff, 2); } -uint16_t RoboClaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init) +uint16_t Roboclaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init) { uint16_t crc = init; @@ -412,7 +412,7 @@ uint16_t RoboClaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init) return crc; } -int RoboClaw::receiveTransaction(e_command cmd, uint8_t *read_buffer, size_t bytes_to_read) +int Roboclaw::receiveTransaction(e_command cmd, uint8_t *read_buffer, size_t bytes_to_read) { if (writeCommand(cmd) != RoboClawError::Success) { return -1; @@ -459,7 +459,7 @@ int RoboClaw::receiveTransaction(e_command cmd, uint8_t *read_buffer, size_t byt return total_bytes_read; } -void RoboClaw::sendTransaction(e_command cmd, uint8_t *write_buffer, size_t bytes_to_write) +void Roboclaw::sendTransaction(e_command cmd, uint8_t *write_buffer, size_t bytes_to_write) { writeCommandWithPayload(cmd, write_buffer, bytes_to_write); int select_status = select(_uart_fd + 1, &_uart_fd_set, nullptr, nullptr, &_uart_fd_timeout); @@ -478,7 +478,7 @@ void RoboClaw::sendTransaction(e_command cmd, uint8_t *write_buffer, size_t byte } } -RoboClaw::RoboClawError RoboClaw::writeCommand(e_command cmd) +Roboclaw::RoboClawError Roboclaw::writeCommand(e_command cmd) { uint8_t buffer[2]; @@ -495,7 +495,7 @@ RoboClaw::RoboClawError RoboClaw::writeCommand(e_command cmd) return RoboClawError::Success; } -RoboClaw::RoboClawError RoboClaw::writeCommandWithPayload(e_command cmd, uint8_t *wbuff, size_t bytes_to_write) +Roboclaw::RoboClawError Roboclaw::writeCommandWithPayload(e_command cmd, uint8_t *wbuff, size_t bytes_to_write) { size_t packet_size = 2 + bytes_to_write + 2; uint8_t buffer[packet_size]; @@ -526,12 +526,12 @@ RoboClaw::RoboClawError RoboClaw::writeCommandWithPayload(e_command cmd, uint8_t return RoboClawError::Success; } -int RoboClaw::custom_command(int argc, char *argv[]) +int Roboclaw::custom_command(int argc, char *argv[]) { return print_usage("unknown command"); } -int RoboClaw::print_usage(const char *reason) +int Roboclaw::print_usage(const char *reason) { if (reason) { PX4_WARN("%s\n", reason); @@ -560,7 +560,7 @@ The command to start this driver is: `$ roboclaw start return 0; } -int RoboClaw::print_status() +int Roboclaw::print_status() { PX4_ERR("Running, Initialized: %f", (double)_initialized); return 0; @@ -568,5 +568,5 @@ int RoboClaw::print_status() extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]) { - return RoboClaw::main(argc, argv); + return Roboclaw::main(argc, argv); } diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/Roboclaw.hpp similarity index 96% rename from src/drivers/roboclaw/RoboClaw.hpp rename to src/drivers/roboclaw/Roboclaw.hpp index 008332ef1390..98ad56e4c7b1 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/Roboclaw.hpp @@ -34,7 +34,7 @@ /** * @file RoboClas.hpp * - * RoboClaw Motor Driver + * Roboclaw Motor Driver * * Product page: https://www.basicmicro.com/motor-controller * Manual: https://downloads.basicmicro.com/docs/roboclaw_user_manual.pdf @@ -55,9 +55,9 @@ #include /** - * This is a driver for the RoboClaw motor controller + * This is a driver for the Roboclaw motor controller */ -class RoboClaw : public ModuleBase, public OutputModuleInterface +class Roboclaw : public ModuleBase, public OutputModuleInterface { public: /** @@ -68,12 +68,12 @@ class RoboClaw : public ModuleBase, public OutputModuleInterface * (selectable on roboclaw) * @param baudRateParam Name of the parameter that holds the baud rate of this serial port */ - RoboClaw(const char *deviceName, const char *baudRateParam); + Roboclaw(const char *deviceName, const char *baudRateParam); /** * deconstructor */ - virtual ~RoboClaw(); + virtual ~Roboclaw(); /** control channels */ enum e_channel { From 00ca59cc3a32c09cf1fe6dc207a1235eeeca5a9a Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 14 Nov 2023 16:37:09 +0100 Subject: [PATCH 19/27] Roboclaw: major cleanup --- msg/WheelEncoders.msg | 5 +- src/drivers/roboclaw/Roboclaw.cpp | 415 ++++++++++++++---------------- src/drivers/roboclaw/Roboclaw.hpp | 189 ++++---------- 3 files changed, 248 insertions(+), 361 deletions(-) diff --git a/msg/WheelEncoders.msg b/msg/WheelEncoders.msg index 368aa9360bad..ab310ac972ff 100644 --- a/msg/WheelEncoders.msg +++ b/msg/WheelEncoders.msg @@ -1,4 +1,5 @@ uint64 timestamp # time since system start (microseconds) -float32[4] wheel_angle #Wheel angle of encoder, if two wheels wheel_angle[0] = right wheel and wheel_angle[1] = left wheel. Else [0][1] = right and [2][3] are left. -float32[4] wheel_speed #Wheel speed of encoder, if two wheels wheel_angle[0] = right wheel and wheel_angle[1] = left wheel. Else [0][1] = right and [2][3] are left. +# Two wheels: 0 left, 1 right +float32[2] wheel_speed # [rad/s] +float32[2] wheel_angle # [rad] diff --git a/src/drivers/roboclaw/Roboclaw.cpp b/src/drivers/roboclaw/Roboclaw.cpp index 23333a0042ef..ea12be76f0fd 100644 --- a/src/drivers/roboclaw/Roboclaw.cpp +++ b/src/drivers/roboclaw/Roboclaw.cpp @@ -41,50 +41,17 @@ * */ - #include "Roboclaw.hpp" -#include -#include -#include -#include -#include #include -#include -#include - -#include -#include -#include -#include - -// The Roboclaw has a serial communication timeout of 10ms. -// 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 5 - -// 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 - -using namespace time_literals; - -Roboclaw::Roboclaw(const char *deviceName, const char *baudRateParam) : +Roboclaw::Roboclaw(const char *device_name, const char *bad_rate_parameter) : OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default) { - strncpy(_storedDeviceName, deviceName, sizeof(_storedDeviceName) - 1); - _storedDeviceName[sizeof(_storedDeviceName) - 1] = '\0'; // Ensure null-termination + strncpy(_stored_device_name, device_name, sizeof(_stored_device_name) - 1); + _stored_device_name[sizeof(_stored_device_name) - 1] = '\0'; // Ensure null-termination - strncpy(_storedBaudRateParam, baudRateParam, sizeof(_storedBaudRateParam) - 1); - _storedBaudRateParam[sizeof(_storedBaudRateParam) - 1] = '\0'; // Ensure null-termination + strncpy(_stored_baud_rate_parameter, bad_rate_parameter, sizeof(_stored_baud_rate_parameter) - 1); + _stored_baud_rate_parameter[sizeof(_stored_baud_rate_parameter) - 1] = '\0'; // Ensure null-termination } Roboclaw::~Roboclaw() @@ -92,13 +59,16 @@ Roboclaw::~Roboclaw() close(_uart_fd); } -int Roboclaw::init() +int Roboclaw::initializeUART() { + // The Roboclaw has a serial communication timeout of 10ms + // Add a little extra to account for timing inaccuracy + static constexpr int TIMEOUT_US = 11_ms; _uart_fd_timeout = { .tv_sec = 0, .tv_usec = TIMEOUT_US }; int32_t baud_rate_parameter_value{0}; int32_t baud_rate_posix{0}; - param_get(param_find(_storedBaudRateParam), &baud_rate_parameter_value); + param_get(param_find(_stored_baud_rate_parameter), &baud_rate_parameter_value); switch (baud_rate_parameter_value) { case 0: // Auto @@ -140,9 +110,9 @@ int Roboclaw::init() } // start serial port - _uart_fd = open(_storedDeviceName, O_RDWR | O_NOCTTY); + _uart_fd = open(_stored_device_name, O_RDWR | O_NOCTTY); - if (_uart_fd < 0) { err(1, "could not open %s", _storedDeviceName); } + if (_uart_fd < 0) { err(1, "could not open %s", _stored_device_name); } int ret = 0; struct termios uart_config {}; @@ -169,47 +139,19 @@ int Roboclaw::init() FD_ZERO(&_uart_fd_set); FD_SET(_uart_fd, &_uart_fd_set); - // Make sure the Roboclaw is actually connected, so I don't just spam errors if it's not. - uint8_t rbuff[6]; // Number of bytes for command 90 status message, read reads status of the roboclaw. - int err_code = receiveTransaction(CMD_READ_STATUS, &rbuff[0], sizeof(rbuff)); + // Make sure the device does respond + static constexpr int READ_STATUS_RESPONSE_SIZE = 6; + uint8_t response_buffer[READ_STATUS_RESPONSE_SIZE]; - if (err_code <= 0) { - PX4_ERR("Shutting down Roboclaw driver."); - return PX4_ERROR; + if (receiveTransaction(Command::ReadStatus, response_buffer, READ_STATUS_RESPONSE_SIZE) < READ_STATUS_RESPONSE_SIZE) { + PX4_ERR("No valid response, stopping driver"); + request_stop(); + return ERROR; } else { PX4_INFO("Successfully connected"); - /* Schedule a cycle to start things. */ - _successfully_connected = true; - return PX4_OK; - } -} - -int Roboclaw::task_spawn(int argc, char *argv[]) -{ - const char *deviceName = argv[1]; - const char *baud_rate_parameter_value = argv[2]; - - Roboclaw *instance = new Roboclaw(deviceName, baud_rate_parameter_value); - - if (instance) { - _object.store(instance); - _task_id = task_id_is_work_queue; - // instance->ScheduleOnInterval(10_ms); // 100 Hz - instance->ScheduleNow(); - return PX4_OK; - - } else { - PX4_ERR("alloc failed"); + return OK; } - - delete instance; - _object.store(nullptr); - _task_id = -1; - - printf("Ending task_spawn"); - - return PX4_ERROR; } bool Roboclaw::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], @@ -219,13 +161,12 @@ bool Roboclaw::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], float right_motor_output = ((float)outputs[0] - 128.0f) / 127.f; if (stop_motors) { - setMotorSpeed(MOTOR_1, 0.f); - setMotorSpeed(MOTOR_2, 0.f); + setMotorSpeed(Motor::Left, 0.f); + setMotorSpeed(Motor::Right, 0.f); } else { - // PX4_ERR("Left and Right motor speed %f %f", (double)left_motor_output, (double)right_motor_output); - setMotorSpeed(MOTOR_1, left_motor_output); - setMotorSpeed(MOTOR_2, right_motor_output); + setMotorSpeed(Motor::Left, left_motor_output); + setMotorSpeed(Motor::Right, right_motor_output); } return true; @@ -242,9 +183,9 @@ void Roboclaw::Run() _mixing_output.update(); - if (!_initialized) { - init(); - _initialized = true; + if (!_uart_initialized) { + initializeUART(); + _uart_initialized = true; } // check for parameter updates @@ -257,121 +198,104 @@ void Roboclaw::Run() } _actuator_armed_sub.update(); - _mixing_output.updateSubscriptions(false); - if (readEncoder() < 0) { + if (readEncoder() != OK) { PX4_ERR("Error reading encoders"); } - } int Roboclaw::readEncoder() { + static constexpr int ENCODER_MESSAGE_SIZE = 10; // response size for ReadEncoderCounters + static constexpr int ENCODER_SPEED_MESSAGE_SIZE = 7; // response size for CMD_READ_SPEED_{1,2} + uint8_t buffer_positon[ENCODER_MESSAGE_SIZE]; uint8_t buffer_speed_right[ENCODER_SPEED_MESSAGE_SIZE]; uint8_t buffer_speed_left[ENCODER_SPEED_MESSAGE_SIZE]; - if (receiveTransaction(CMD_READ_BOTH_ENCODERS, buffer_positon, ENCODER_MESSAGE_SIZE) < ENCODER_MESSAGE_SIZE) { - return -1; + if (receiveTransaction(Command::ReadSpeedMotor2, buffer_speed_left, + ENCODER_SPEED_MESSAGE_SIZE) < ENCODER_SPEED_MESSAGE_SIZE) { + return ERROR; } - if (receiveTransaction(CMD_READ_SPEED_1, buffer_speed_right, ENCODER_SPEED_MESSAGE_SIZE) < ENCODER_SPEED_MESSAGE_SIZE) { - return -1; + if (receiveTransaction(Command::ReadSpeedMotor1, buffer_speed_right, + ENCODER_SPEED_MESSAGE_SIZE) < ENCODER_SPEED_MESSAGE_SIZE) { + return ERROR; } - if (receiveTransaction(CMD_READ_SPEED_2, buffer_speed_left, ENCODER_SPEED_MESSAGE_SIZE) < ENCODER_SPEED_MESSAGE_SIZE) { - return -1; + if (receiveTransaction(Command::ReadEncoderCounters, buffer_positon, ENCODER_MESSAGE_SIZE) < ENCODER_MESSAGE_SIZE) { + return ERROR; } - int32_t position_right = reverseInt32(&buffer_positon[0]); - int32_t position_left = reverseInt32(&buffer_positon[4]); - int32_t speed_right = reverseInt32(&buffer_speed_right[0]); - int32_t speed_left = reverseInt32(&buffer_speed_left[0]); + int32_t speed_left = swapBytesInt32(&buffer_speed_left[0]); + int32_t speed_right = swapBytesInt32(&buffer_speed_right[0]); + int32_t position_left = swapBytesInt32(&buffer_positon[4]); + int32_t position_right = swapBytesInt32(&buffer_positon[0]); wheel_encoders_s wheel_encoders{}; - wheel_encoders.wheel_angle[0] = static_cast(position_right) / _param_rbclw_counts_rev.get() * M_TWOPI_F; - wheel_encoders.wheel_angle[1] = static_cast(position_left) / _param_rbclw_counts_rev.get() * M_TWOPI_F; - wheel_encoders.wheel_speed[0] = static_cast(speed_right) / _param_rbclw_counts_rev.get() * M_TWOPI_F; - wheel_encoders.wheel_speed[1] = static_cast(speed_left) / _param_rbclw_counts_rev.get() * M_TWOPI_F; + wheel_encoders.wheel_speed[0] = static_cast(speed_left) / _param_rbclw_counts_rev.get() * M_TWOPI_F; + wheel_encoders.wheel_speed[1] = static_cast(speed_right) / _param_rbclw_counts_rev.get() * M_TWOPI_F; + wheel_encoders.wheel_angle[0] = static_cast(position_left) / _param_rbclw_counts_rev.get() * M_TWOPI_F; + wheel_encoders.wheel_angle[1] = static_cast(position_right) / _param_rbclw_counts_rev.get() * M_TWOPI_F; wheel_encoders.timestamp = hrt_absolute_time(); _wheel_encoders_pub.publish(wheel_encoders); - return 1; -} - -int32_t Roboclaw::reverseInt32(uint8_t *buffer) -{ - return (buffer[0] << 24) - | (buffer[1] << 16) - | (buffer[2] << 8) - | buffer[3]; + return OK; } -void Roboclaw::setMotorSpeed(e_motor motor, float value) +void Roboclaw::setMotorSpeed(Motor motor, float value) { - e_command command; + Command command; // send command - if (motor == MOTOR_1) { + if (motor == Motor::Left) { if (value > 0) { - command = CMD_DRIVE_FWD_1; + command = Command::DriveForwardMotor1; } else { - command = CMD_DRIVE_REV_1; + command = Command::DriveBackwardsMotor1; } - } else if (motor == MOTOR_2) { + } else if (motor == Motor::Right) { if (value > 0) { - command = CMD_DRIVE_FWD_2; + command = Command::DriveForwardMotor2; } else { - command = CMD_DRIVE_REV_2; + command = Command::DriveBackwardsMotor2; } } else { return; } - _sendUnsigned7Bit(command, value); + sendUnsigned7Bit(command, value); } -void Roboclaw::setMotorDutyCycle(e_motor motor, float value) +void Roboclaw::setMotorDutyCycle(Motor motor, float value) { - e_command command; + Command command; // send command - if (motor == MOTOR_1) { - command = CMD_SIGNED_DUTYCYCLE_1; + if (motor == Motor::Left) { + command = Command::DutyCycleMotor1; - } else if (motor == MOTOR_2) { - command = CMD_SIGNED_DUTYCYCLE_2; + } else if (motor == Motor::Right) { + command = Command::DutyCycleMotor2; } else { return; } - return _sendSigned16Bit(command, value); -} - -void Roboclaw::drive(float value) -{ - e_command command = value >= 0 ? CMD_DRIVE_FWD_MIX : CMD_DRIVE_REV_MIX; - _sendUnsigned7Bit(command, value); -} - -void Roboclaw::turn(float value) -{ - e_command command = value >= 0 ? CMD_TURN_LEFT : CMD_TURN_RIGHT; - _sendUnsigned7Bit(command, value); + return sendSigned16Bit(command, value); } void Roboclaw::resetEncoders() { - sendTransaction(CMD_RESET_ENCODERS, nullptr, 0); + sendTransaction(Command::ResetEncoders, nullptr, 0); } -void Roboclaw::_sendUnsigned7Bit(e_command command, float data) +void Roboclaw::sendUnsigned7Bit(Command command, float data) { data = fabs(data); @@ -383,41 +307,104 @@ void Roboclaw::_sendUnsigned7Bit(e_command command, float data) sendTransaction(command, &byte, 1); } -void Roboclaw::_sendSigned16Bit(e_command command, float data) +void Roboclaw::sendSigned16Bit(Command command, float data) { - int16_t duty = math::constrain(data, -1.f, 1.f) * INT16_MAX; + int16_t value = math::constrain(data, -1.f, 1.f) * INT16_MAX; uint8_t buff[2]; - buff[0] = (duty >> 8) & 0xFF; // High byte - buff[1] = duty & 0xFF; // Low byte + buff[0] = (value >> 8) & 0xFF; // High byte + buff[1] = value & 0xFF; // Low byte sendTransaction(command, (uint8_t *) &buff, 2); } -uint16_t Roboclaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init) +int Roboclaw::sendTransaction(Command cmd, uint8_t *write_buffer, size_t bytes_to_write) { - uint16_t crc = init; + if (writeCommandWithPayload(cmd, write_buffer, bytes_to_write) != OK) { + return ERROR; + } - for (size_t byte = 0; byte < n; byte++) { - crc = crc ^ (((uint16_t) buf[byte]) << 8); + return readAcknowledgement(); +} - for (uint8_t bit = 0; bit < 8; bit++) { - if (crc & 0x8000) { - crc = (crc << 1) ^ 0x1021; +int Roboclaw::writeCommandWithPayload(Command command, uint8_t *wbuff, size_t bytes_to_write) +{ + size_t packet_size = 2 + bytes_to_write + 2; + uint8_t buffer[packet_size]; - } else { - crc = crc << 1; - } - } + // Add address + command ID + buffer[0] = (uint8_t) _param_rbclw_address.get(); + buffer[1] = static_cast(command); + + // Add payload + if (bytes_to_write > 0 && wbuff) { + memcpy(&buffer[2], wbuff, bytes_to_write); } - return crc; + // Add checksum + uint16_t sum = _calcCRC(buffer, packet_size - 2); + buffer[packet_size - 2] = (sum >> 8) & 0xFF; + buffer[packet_size - 1] = sum & 0xFFu; + + // Write to device + size_t bytes_written = write(_uart_fd, buffer, packet_size); + + // Not all bytes sent + if (bytes_written < packet_size) { + PX4_ERR("Only wrote %d out of %d bytes", bytes_written, bytes_to_write); + return ERROR; + } + + return OK; } -int Roboclaw::receiveTransaction(e_command cmd, uint8_t *read_buffer, size_t bytes_to_read) +int Roboclaw::readAcknowledgement() { - if (writeCommand(cmd) != RoboClawError::Success) { - return -1; + int select_status = select(_uart_fd + 1, &_uart_fd_set, nullptr, nullptr, &_uart_fd_timeout); + + if (select_status <= 0) { + PX4_ERR("ACK timeout"); + return ERROR; } + uint8_t acknowledgement{0}; + int bytes_read = read(_uart_fd, &acknowledgement, 1); + + if ((bytes_read != 1) || (acknowledgement != 0xFF)) { + PX4_ERR("ACK wrong"); + return ERROR; + } + + return OK; +} + +int Roboclaw::receiveTransaction(Command command, uint8_t *read_buffer, size_t bytes_to_read) +{ + if (writeCommand(command) != OK) { + return ERROR; + } + + return readResponse(command, read_buffer, bytes_to_read); +} + +int Roboclaw::writeCommand(Command command) +{ + uint8_t buffer[2]; + + // Just address + command ID + buffer[0] = (uint8_t)_param_rbclw_address.get(); + buffer[1] = static_cast(command); + + size_t bytes_written = write(_uart_fd, buffer, 2); + + if (bytes_written < 2) { + PX4_ERR("Only wrote %d out of %d bytes", bytes_written, 2); + return ERROR; + } + + return OK; +} + +int Roboclaw::readResponse(Command command, uint8_t *read_buffer, size_t bytes_to_read) +{ size_t total_bytes_read = 0; while (total_bytes_read < bytes_to_read) { @@ -425,14 +412,14 @@ int Roboclaw::receiveTransaction(e_command cmd, uint8_t *read_buffer, size_t byt if (select_status <= 0) { PX4_ERR("Select timeout %d\n", select_status); - return -1; + return ERROR; } int bytes_read = read(_uart_fd, &read_buffer[total_bytes_read], bytes_to_read - total_bytes_read); if (bytes_read <= 0) { PX4_ERR("Read timeout %d\n", select_status); - return -1; + return ERROR; } total_bytes_read += bytes_read; @@ -440,90 +427,77 @@ int Roboclaw::receiveTransaction(e_command cmd, uint8_t *read_buffer, size_t byt if (total_bytes_read < 2) { PX4_ERR("Too short payload received\n"); - return -1; + return ERROR; } - // Verify checksum + // Verify response checksum uint8_t address = static_cast(_param_rbclw_address.get()); - uint8_t command = static_cast(cmd); - uint16_t checksum_calc = _calcCRC(&address, 1); // address - checksum_calc = _calcCRC(&command, 1, checksum_calc); // command - checksum_calc = _calcCRC(read_buffer, total_bytes_read - 2, checksum_calc); // received payload - uint16_t checksum_recv = (read_buffer[total_bytes_read - 2] << 8) + read_buffer[total_bytes_read - 1]; + uint8_t command_byte = static_cast(command); + uint16_t crc_calculated = _calcCRC(&address, 1); // address + crc_calculated = _calcCRC(&command_byte, 1, crc_calculated); // command + crc_calculated = _calcCRC(read_buffer, total_bytes_read - 2, crc_calculated); // received payload + uint16_t crc_received = (read_buffer[total_bytes_read - 2] << 8) + read_buffer[total_bytes_read - 1]; - if (checksum_calc != checksum_recv) { + if (crc_calculated != crc_received) { PX4_ERR("Checksum mismatch\n"); - return -1; + return ERROR; } return total_bytes_read; } -void Roboclaw::sendTransaction(e_command cmd, uint8_t *write_buffer, size_t bytes_to_write) +uint16_t Roboclaw::_calcCRC(const uint8_t *buffer, size_t bytes, uint16_t init) { - writeCommandWithPayload(cmd, write_buffer, bytes_to_write); - int select_status = select(_uart_fd + 1, &_uart_fd_set, nullptr, nullptr, &_uart_fd_timeout); + uint16_t crc = init; - if (select_status <= 0) { - PX4_ERR("ACK timeout"); - return; - } + for (size_t byte = 0; byte < bytes; byte++) { + crc = crc ^ (((uint16_t) buffer[byte]) << 8); - uint8_t acknowledgement{0}; - int bytes_read = read(_uart_fd, &acknowledgement, 1); + for (uint8_t bit = 0; bit < 8; bit++) { + if (crc & 0x8000) { + crc = (crc << 1) ^ 0x1021; - if ((bytes_read != 1) || (acknowledgement != 0xFF)) { - PX4_ERR("ACK wrong"); - return; + } else { + crc = crc << 1; + } + } } + + return crc; } -Roboclaw::RoboClawError Roboclaw::writeCommand(e_command cmd) +int32_t Roboclaw::swapBytesInt32(uint8_t *buffer) { - uint8_t buffer[2]; - - buffer[0] = (uint8_t)_param_rbclw_address.get(); - buffer[1] = cmd; - - size_t count = write(_uart_fd, buffer, 2); - - if (count < 2) { - PX4_ERR("Only wrote %d out of %zu bytes", count, 2); - return RoboClawError::WriteError; - } - - return RoboClawError::Success; + return (buffer[0] << 24) + | (buffer[1] << 16) + | (buffer[2] << 8) + | buffer[3]; } -Roboclaw::RoboClawError Roboclaw::writeCommandWithPayload(e_command cmd, uint8_t *wbuff, size_t bytes_to_write) +int Roboclaw::task_spawn(int argc, char *argv[]) { - size_t packet_size = 2 + bytes_to_write + 2; - uint8_t buffer[packet_size]; + const char *device_name = argv[1]; + const char *baud_rate_parameter_value = argv[2]; - // Add address + command ID - buffer[0] = (uint8_t) _param_rbclw_address.get(); - buffer[1] = cmd; + Roboclaw *instance = new Roboclaw(device_name, baud_rate_parameter_value); - // Add payload - if (bytes_to_write > 0 && wbuff) { - memcpy(&buffer[2], wbuff, bytes_to_write); - } + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + instance->ScheduleNow(); + return OK; - // Add checksum - uint16_t sum = _calcCRC(buffer, packet_size - 2); - buffer[packet_size - 2] = (sum >> 8) & 0xFF; - buffer[packet_size - 1] = sum & 0xFFu; + } else { + PX4_ERR("alloc failed"); + } - // Write to device - size_t count = write(_uart_fd, buffer, packet_size); + delete instance; + _object.store(nullptr); + _task_id = -1; - // Not all bytes sent - if (count < packet_size) { - PX4_ERR("Only wrote %d out of %d bytes", count, bytes_to_write); - return RoboClawError::WriteError; - } + printf("Ending task_spawn"); - return RoboClawError::Success; + return ERROR; } int Roboclaw::custom_command(int argc, char *argv[]) @@ -562,7 +536,6 @@ The command to start this driver is: `$ roboclaw start int Roboclaw::print_status() { - PX4_ERR("Running, Initialized: %f", (double)_initialized); return 0; } diff --git a/src/drivers/roboclaw/Roboclaw.hpp b/src/drivers/roboclaw/Roboclaw.hpp index 98ad56e4c7b1..26e7f9e628b7 100644 --- a/src/drivers/roboclaw/Roboclaw.hpp +++ b/src/drivers/roboclaw/Roboclaw.hpp @@ -32,9 +32,9 @@ ****************************************************************************/ /** - * @file RoboClas.hpp + * @file Roboclaw.hpp * - * Roboclaw Motor Driver + * Roboclaw motor control driver * * Product page: https://www.basicmicro.com/motor-controller * Manual: https://downloads.basicmicro.com/docs/roboclaw_user_manual.pdf @@ -54,173 +54,86 @@ #include #include -/** - * This is a driver for the Roboclaw motor controller - */ class Roboclaw : public ModuleBase, public OutputModuleInterface { public: /** - * constructor - * @param deviceName the name of the - * serial port e.g. "/dev/ttyS2" - * @param address the adddress of the motor - * (selectable on roboclaw) - * @param baudRateParam Name of the parameter that holds the baud rate of this serial port - */ - Roboclaw(const char *deviceName, const char *baudRateParam); - - /** - * deconstructor + * @param device_name Name of the serial port e.g. "/dev/ttyS2" + * @param bad_rate_parameter Name of the parameter that holds the baud rate of this serial port */ + Roboclaw(const char *device_name, const char *bad_rate_parameter); virtual ~Roboclaw(); - /** control channels */ - enum e_channel { - CH_VOLTAGE_LEFT = 0, - CH_VOLTAGE_RIGHT - }; - - /** motors */ - enum e_motor { - MOTOR_1 = 0, - MOTOR_2 + enum class Motor { + Left = 0, + Right = 1 }; - /** error handeling*/ - enum class RoboClawError { - Success, - WriteError, - ReadError, - ChecksumError, - ChecksumMismatch, - UnexpectedError, - ReadTimeout - }; - - /** @see ModuleBase */ - static int task_spawn(int argc, char *argv[]); - - /** @see ModuleBase */ - static int custom_command(int argc, char *argv[]); - - /** @see ModuleBase */ - static int print_usage(const char *reason = nullptr); - - /** @see ModuleBase::print_status() */ - int print_status() override; + static int task_spawn(int argc, char *argv[]); ///< @see ModuleBase + static int custom_command(int argc, char *argv[]); ///< @see ModuleBase + static int print_usage(const char *reason = nullptr); ///< @see ModuleBase + int print_status() override; ///< @see ModuleBase void Run() override; - int init(); - + /** @see OutputModuleInterface */ bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) override; - /** - * set the speed of a motor, rev/sec - */ - void setMotorSpeed(e_motor motor, float value); - - /** - * set the duty cycle of a motor - */ - void setMotorDutyCycle(e_motor motor, float value); - - /** - * Drive both motors. +1 = full forward, -1 = full backward - */ - void drive(float value); - - /** - * Turn. +1 = full right, -1 = full left - */ - void turn(float value); - - /** - * reset the encoders - * @return status - */ - void resetEncoders(); - - /** - * read data from serial - */ + void setMotorSpeed(Motor motor, float value); ///< rev/sec + void setMotorDutyCycle(Motor motor, float value); int readEncoder(); + void resetEncoders(); private: - static constexpr int MAX_ACTUATORS = 2; + enum class Command : uint8_t { + ReadStatus = 90, + + DriveForwardMotor1 = 0, + DriveBackwardsMotor1 = 1, + DriveForwardMotor2 = 4, + DriveBackwardsMotor2 = 5, + DutyCycleMotor1 = 32, + DutyCycleMotor2 = 33, + + ReadSpeedMotor1 = 18, + ReadSpeedMotor2 = 19, + ResetEncoders = 20, + ReadEncoderCounters = 78, + }; + static constexpr int MAX_ACTUATORS = 2; MixingOutput _mixing_output{"ROBOCLAW", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false}; - char _storedDeviceName[256]; // Adjust size as necessary - char _storedBaudRateParam[256]; // Adjust size as necessary - - int _timeout_counter = 0; - - bool _successfully_connected{false}; - - // commands - // We just list the commands we want from the manual here. - enum e_command { + uORB::SubscriptionData _actuator_armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; + uORB::Publication _wheel_encoders_pub{ORB_ID(wheel_encoders)}; - // simple - CMD_DRIVE_FWD_1 = 0, - CMD_DRIVE_REV_1 = 1, - CMD_DRIVE_FWD_2 = 4, - CMD_DRIVE_REV_2 = 5, + char _stored_device_name[256]; // Adjust size as necessary + char _stored_baud_rate_parameter[256]; // Adjust size as necessary - CMD_DRIVE_FWD_MIX = 8, - CMD_DRIVE_REV_MIX = 9, - CMD_TURN_RIGHT = 10, - CMD_TURN_LEFT = 11, + void sendUnsigned7Bit(Command command, float data); + void sendSigned16Bit(Command command, float data); - // encoder commands - CMD_READ_ENCODER_1 = 16, - CMD_READ_ENCODER_2 = 17, - CMD_READ_SPEED_1 = 18, - CMD_READ_SPEED_2 = 19, - CMD_RESET_ENCODERS = 20, - CMD_READ_BOTH_ENCODERS = 78, - CMD_READ_BOTH_SPEEDS = 79, + // Roboclaw protocol + int sendTransaction(Command cmd, uint8_t *write_buffer, size_t bytes_to_write); + int writeCommandWithPayload(Command cmd, uint8_t *wbuff, size_t bytes_to_write); + int readAcknowledgement(); - // advanced motor control - CMD_READ_SPEED_HIRES_1 = 30, - CMD_READ_SPEED_HIRES_2 = 31, - CMD_SIGNED_DUTYCYCLE_1 = 32, - CMD_SIGNED_DUTYCYCLE_2 = 33, + int receiveTransaction(Command cmd, uint8_t *read_buffer, size_t bytes_to_read); + int writeCommand(Command cmd); + int readResponse(Command command, uint8_t *read_buffer, size_t bytes_to_read); - CMD_READ_STATUS = 90 - }; + static uint16_t _calcCRC(const uint8_t *buf, size_t n, uint16_t init = 0); + int32_t swapBytesInt32(uint8_t *buffer); + // UART handling + int initializeUART(); + bool _uart_initialized{false}; int _uart_fd{0}; fd_set _uart_fd_set; struct timeval _uart_fd_timeout; - uORB::SubscriptionData _actuator_armed_sub{ORB_ID(actuator_armed)}; - uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - - matrix::Vector2f _motor_control{0.0f, 0.0f}; - - uORB::Publication _wheel_encoders_pub {ORB_ID(wheel_encoders)}; - - void _parameters_update(); - - static uint16_t _calcCRC(const uint8_t *buf, size_t n, uint16_t init = 0); - void _sendUnsigned7Bit(e_command command, float data); - void _sendSigned16Bit(e_command command, float data); - - int32_t reverseInt32(uint8_t *buffer); - - bool _initialized{false}; - - RoboClawError writeCommand(e_command cmd); - RoboClawError writeCommandWithPayload(e_command cmd, uint8_t *wbuff, size_t bytes_to_write); - - void sendTransaction(e_command cmd, uint8_t *write_buffer, size_t bytes_to_write); - int receiveTransaction(e_command cmd, uint8_t *read_buffer, size_t bytes_to_read); - - DEFINE_PARAMETERS( (ParamInt) _param_rbclw_address, (ParamInt) _param_rbclw_counts_rev From aa1366ddc797a1cc01f05d8f5f0e6f5a10ab696f Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Wed, 15 Nov 2023 15:25:20 +0100 Subject: [PATCH 20/27] Roboclaw: Consistent Left & Right naming convertion with Differential Drive class --- msg/WheelEncoders.msg | 2 +- src/drivers/roboclaw/Roboclaw.cpp | 10 +++++----- src/drivers/roboclaw/Roboclaw.hpp | 4 ++-- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/msg/WheelEncoders.msg b/msg/WheelEncoders.msg index ab310ac972ff..a4f3955dcb39 100644 --- a/msg/WheelEncoders.msg +++ b/msg/WheelEncoders.msg @@ -1,5 +1,5 @@ uint64 timestamp # time since system start (microseconds) -# Two wheels: 0 left, 1 right +# Two wheels: 0 right, 1 left float32[2] wheel_speed # [rad/s] float32[2] wheel_angle # [rad] diff --git a/src/drivers/roboclaw/Roboclaw.cpp b/src/drivers/roboclaw/Roboclaw.cpp index ea12be76f0fd..28e0cee05864 100644 --- a/src/drivers/roboclaw/Roboclaw.cpp +++ b/src/drivers/roboclaw/Roboclaw.cpp @@ -157,8 +157,8 @@ int Roboclaw::initializeUART() bool Roboclaw::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { - float left_motor_output = -((float)outputs[1] - 128.0f) / 127.f; float right_motor_output = ((float)outputs[0] - 128.0f) / 127.f; + float left_motor_output = -((float)outputs[1] - 128.0f) / 127.f; if (stop_motors) { setMotorSpeed(Motor::Left, 0.f); @@ -234,10 +234,10 @@ int Roboclaw::readEncoder() int32_t position_right = swapBytesInt32(&buffer_positon[0]); wheel_encoders_s wheel_encoders{}; - wheel_encoders.wheel_speed[0] = static_cast(speed_left) / _param_rbclw_counts_rev.get() * M_TWOPI_F; - wheel_encoders.wheel_speed[1] = static_cast(speed_right) / _param_rbclw_counts_rev.get() * M_TWOPI_F; - wheel_encoders.wheel_angle[0] = static_cast(position_left) / _param_rbclw_counts_rev.get() * M_TWOPI_F; - wheel_encoders.wheel_angle[1] = static_cast(position_right) / _param_rbclw_counts_rev.get() * M_TWOPI_F; + wheel_encoders.wheel_speed[0] = static_cast(speed_right) / _param_rbclw_counts_rev.get() * M_TWOPI_F; + wheel_encoders.wheel_speed[1] = static_cast(speed_left) / _param_rbclw_counts_rev.get() * M_TWOPI_F; + wheel_encoders.wheel_angle[0] = static_cast(position_right) / _param_rbclw_counts_rev.get() * M_TWOPI_F; + wheel_encoders.wheel_angle[1] = static_cast(position_left) / _param_rbclw_counts_rev.get() * M_TWOPI_F; wheel_encoders.timestamp = hrt_absolute_time(); _wheel_encoders_pub.publish(wheel_encoders); diff --git a/src/drivers/roboclaw/Roboclaw.hpp b/src/drivers/roboclaw/Roboclaw.hpp index 26e7f9e628b7..768198be4e9b 100644 --- a/src/drivers/roboclaw/Roboclaw.hpp +++ b/src/drivers/roboclaw/Roboclaw.hpp @@ -65,8 +65,8 @@ class Roboclaw : public ModuleBase, public OutputModuleInterface virtual ~Roboclaw(); enum class Motor { - Left = 0, - Right = 1 + Right = 0, + Left = 1 }; static int task_spawn(int argc, char *argv[]); ///< @see ModuleBase From 74e103b385aa08361d1a22d5ff4ba39a0524d4f8 Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Wed, 15 Nov 2023 17:00:51 +0100 Subject: [PATCH 21/27] Roboclaw: Updated Airframe and fixed left and right mapping error --- .../airframes/50003_aion_robotics_r1_rover | 20 +++++++++-------- src/drivers/roboclaw/Roboclaw.cpp | 22 +++++++++---------- src/modules/control_allocator/module.yaml | 4 ++-- 3 files changed, 24 insertions(+), 22 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover index c8940fcc77c7..2fe66d9876e9 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover @@ -13,6 +13,8 @@ . ${R}etc/init.d/rc.rover_defaults + + param set-default BAT1_N_CELLS 4 param set-default EKF2_GBIAS_INIT 0.01 @@ -46,15 +48,15 @@ param set-default GND_SPEED_THR_SC 1 param set-default NAV_ACC_RAD 0.5 -# Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians -param set-default GND_MAX_ANG 3.1415 - # Set geometry & output configration param set-default CA_AIRFRAME 6 param set-default CA_R_REV 3 -param set-default PWM_MAIN_FUNC1 101 -param set-default PWM_MAIN_FUNC2 102 -param set-default PWM_MAIN_DIS1 1500 -param set-default PWM_MAIN_DIS2 1500 -param set-default PWM_MAIN_TIM0 50 -param set-default PWM_MAIN_TIM1 50 + + +param set-default RBCLW_ADDRESS 130 +param set-default RBCLW_SER_CFG 202 +param set-default ROBOCLAW_FUNC1 101 +param set-default ROBOCLAW_FUNC2 102 +param set-default ROBOCLAW_REV 1 +param set-default SENS_EN_INA226 0 +param set-default SER_GPS2_BAUD 57600 diff --git a/src/drivers/roboclaw/Roboclaw.cpp b/src/drivers/roboclaw/Roboclaw.cpp index 28e0cee05864..d4aeeee5d3ee 100644 --- a/src/drivers/roboclaw/Roboclaw.cpp +++ b/src/drivers/roboclaw/Roboclaw.cpp @@ -158,15 +158,15 @@ bool Roboclaw::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs, unsigned num_control_groups_updated) { float right_motor_output = ((float)outputs[0] - 128.0f) / 127.f; - float left_motor_output = -((float)outputs[1] - 128.0f) / 127.f; + float left_motor_output = ((float)outputs[1] - 128.0f) / 127.f; if (stop_motors) { - setMotorSpeed(Motor::Left, 0.f); setMotorSpeed(Motor::Right, 0.f); + setMotorSpeed(Motor::Left, 0.f); } else { - setMotorSpeed(Motor::Left, left_motor_output); setMotorSpeed(Motor::Right, right_motor_output); + setMotorSpeed(Motor::Left, left_motor_output); } return true; @@ -214,12 +214,12 @@ int Roboclaw::readEncoder() uint8_t buffer_speed_right[ENCODER_SPEED_MESSAGE_SIZE]; uint8_t buffer_speed_left[ENCODER_SPEED_MESSAGE_SIZE]; - if (receiveTransaction(Command::ReadSpeedMotor2, buffer_speed_left, + if (receiveTransaction(Command::ReadSpeedMotor1, buffer_speed_right, ENCODER_SPEED_MESSAGE_SIZE) < ENCODER_SPEED_MESSAGE_SIZE) { return ERROR; } - if (receiveTransaction(Command::ReadSpeedMotor1, buffer_speed_right, + if (receiveTransaction(Command::ReadSpeedMotor2, buffer_speed_left, ENCODER_SPEED_MESSAGE_SIZE) < ENCODER_SPEED_MESSAGE_SIZE) { return ERROR; } @@ -228,10 +228,10 @@ int Roboclaw::readEncoder() return ERROR; } - int32_t speed_left = swapBytesInt32(&buffer_speed_left[0]); int32_t speed_right = swapBytesInt32(&buffer_speed_right[0]); - int32_t position_left = swapBytesInt32(&buffer_positon[4]); + int32_t speed_left = swapBytesInt32(&buffer_speed_left[0]); int32_t position_right = swapBytesInt32(&buffer_positon[0]); + int32_t position_left = swapBytesInt32(&buffer_positon[4]); wheel_encoders_s wheel_encoders{}; wheel_encoders.wheel_speed[0] = static_cast(speed_right) / _param_rbclw_counts_rev.get() * M_TWOPI_F; @@ -249,7 +249,7 @@ void Roboclaw::setMotorSpeed(Motor motor, float value) Command command; // send command - if (motor == Motor::Left) { + if (motor == Motor::Right) { if (value > 0) { command = Command::DriveForwardMotor1; @@ -257,7 +257,7 @@ void Roboclaw::setMotorSpeed(Motor motor, float value) command = Command::DriveBackwardsMotor1; } - } else if (motor == Motor::Right) { + } else if (motor == Motor::Left) { if (value > 0) { command = Command::DriveForwardMotor2; @@ -277,10 +277,10 @@ void Roboclaw::setMotorDutyCycle(Motor motor, float value) Command command; // send command - if (motor == Motor::Left) { + if (motor == Motor::Right) { command = Command::DutyCycleMotor1; - } else if (motor == Motor::Right) { + } else if (motor == Motor::Left) { command = Command::DutyCycleMotor2; } else { diff --git a/src/modules/control_allocator/module.yaml b/src/modules/control_allocator/module.yaml index 2c506cc4e270..8683d7477d95 100644 --- a/src/modules/control_allocator/module.yaml +++ b/src/modules/control_allocator/module.yaml @@ -961,10 +961,10 @@ mixer: actuators: - actuator_type: 'motor' instances: - - name: 'Left Motor' - position: [ 0, -1., 0 ] - name: 'Right Motor' position: [ 0, 1., 0 ] + - name: 'Left Motor' + position: [ 0, -1., 0 ] 7: # Motors (6DOF) actuators: From ea96cc599da5b5f077eed5768c33eb822472d462 Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Wed, 15 Nov 2023 17:13:44 +0100 Subject: [PATCH 22/27] Roboclaw: Accidentally removed a parameter --- .../init.d/airframes/50003_aion_robotics_r1_rover | 3 +++ 1 file changed, 3 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover index 2fe66d9876e9..79c2dfdfd8bb 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover @@ -48,6 +48,9 @@ param set-default GND_SPEED_THR_SC 1 param set-default NAV_ACC_RAD 0.5 +# Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians +param set-default GND_MAX_ANG 3.1415 + # Set geometry & output configration param set-default CA_AIRFRAME 6 param set-default CA_R_REV 3 From 6939bfd9e42afefd0885df8f77948f08914feafe Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Mon, 20 Nov 2023 10:02:17 +0100 Subject: [PATCH 23/27] Roboclaw: Changes in r1 airframe, removed hardcoded port configurations --- .../init.d/airframes/50003_aion_robotics_r1_rover | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover index 79c2dfdfd8bb..116a991b547a 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover @@ -56,10 +56,7 @@ param set-default CA_AIRFRAME 6 param set-default CA_R_REV 3 -param set-default RBCLW_ADDRESS 130 -param set-default RBCLW_SER_CFG 202 +param set-default RBCLW_ADDRESS 128 param set-default ROBOCLAW_FUNC1 101 param set-default ROBOCLAW_FUNC2 102 param set-default ROBOCLAW_REV 1 -param set-default SENS_EN_INA226 0 -param set-default SER_GPS2_BAUD 57600 From ce8123a00c63233a8b89ec548a4cc133555a77da Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Mon, 20 Nov 2023 11:51:44 +0100 Subject: [PATCH 24/27] Roboclaw: Updated yaml file to support Roboclaw Driver in QGC --- src/drivers/roboclaw/module.yaml | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/drivers/roboclaw/module.yaml b/src/drivers/roboclaw/module.yaml index e9dc4e9ddbb3..1eaa38461fd4 100644 --- a/src/drivers/roboclaw/module.yaml +++ b/src/drivers/roboclaw/module.yaml @@ -5,6 +5,17 @@ serial_config: name: RBCLW_SER_CFG group: Roboclaw +actuator_output: + output_groups: + - param_prefix: ROBOCLAW + channel_label: 'Channel' + standard_params: + disarmed: { min: 128, max: 128, default: 128 } + min: { min: 1, max: 128, default: 1 } + max: { min: 128, max: 256, default: 256 } + failsafe: { min: 0, max: 257 } + num_channels: 2 + parameters: - group: Roboclaw Driver definitions: From c9bf3e2ce04bd1548694d0dd65fd2a539d5aa3d6 Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Tue, 28 Nov 2023 13:28:24 +0100 Subject: [PATCH 25/27] Roboclaw: Fix CI pr issue --- src/drivers/roboclaw/module.yaml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/drivers/roboclaw/module.yaml b/src/drivers/roboclaw/module.yaml index 1eaa38461fd4..5406bcbb8f25 100644 --- a/src/drivers/roboclaw/module.yaml +++ b/src/drivers/roboclaw/module.yaml @@ -35,11 +35,11 @@ parameters: min: 128 max: 135 values: - 128: 0x80 - 129: 0x81 - 130: 0x82 - 131: 0x83 - 132: 0x84 - 133: 0x85 - 134: 0x86 - 135: 0x87 + 128: '0x80' + 129: '0x81' + 130: '0x82' + 131: '0x83' + 132: '0x84' + 133: '0x85' + 134: '0x86' + 135: '0x87' From 2baf4a6afff63caf97c73642289e26e7861117d0 Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Tue, 28 Nov 2023 13:57:05 +0100 Subject: [PATCH 26/27] Roboclaw: Fixed issue where parameters had different prefixes --- .../init.d/airframes/50003_aion_robotics_r1_rover | 6 +++--- src/drivers/roboclaw/module.yaml | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover index 116a991b547a..4cc66c908a52 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover @@ -57,6 +57,6 @@ param set-default CA_R_REV 3 param set-default RBCLW_ADDRESS 128 -param set-default ROBOCLAW_FUNC1 101 -param set-default ROBOCLAW_FUNC2 102 -param set-default ROBOCLAW_REV 1 +param set-default RBCLW_FUNC1 101 +param set-default RBCLW_FUNC2 102 +param set-default RBCLW_REV 1 diff --git a/src/drivers/roboclaw/module.yaml b/src/drivers/roboclaw/module.yaml index 5406bcbb8f25..1a4b58691c3c 100644 --- a/src/drivers/roboclaw/module.yaml +++ b/src/drivers/roboclaw/module.yaml @@ -7,7 +7,7 @@ serial_config: actuator_output: output_groups: - - param_prefix: ROBOCLAW + - param_prefix: RBCLW channel_label: 'Channel' standard_params: disarmed: { min: 128, max: 128, default: 128 } From f06b9314eb672dc13107f14f9d86f6091d0e0d8c Mon Sep 17 00:00:00 2001 From: PerFrivik Date: Tue, 28 Nov 2023 14:14:40 +0100 Subject: [PATCH 27/27] Roboclaw: Updated parameter prefix for roboclaw output module --- src/drivers/roboclaw/Roboclaw.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/roboclaw/Roboclaw.hpp b/src/drivers/roboclaw/Roboclaw.hpp index 768198be4e9b..c58ef1bf5eb6 100644 --- a/src/drivers/roboclaw/Roboclaw.hpp +++ b/src/drivers/roboclaw/Roboclaw.hpp @@ -103,7 +103,7 @@ class Roboclaw : public ModuleBase, public OutputModuleInterface }; static constexpr int MAX_ACTUATORS = 2; - MixingOutput _mixing_output{"ROBOCLAW", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false}; + MixingOutput _mixing_output{"RBCLW", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false}; uORB::SubscriptionData _actuator_armed_sub{ORB_ID(actuator_armed)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};