From a857df88e4b301c7cac41248b37d0e3d9de5d6f3 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 14 Dec 2021 16:48:22 +0100 Subject: [PATCH] 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 d0ce86c236..82276e4396 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 0000000000..f70f3c5385 --- /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 81e5f694df..b501b1ecb3 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 44c9ca8d6b..a3483c25a5 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");