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 ed7a7912c8..1fe870493f 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 @@ -54,14 +54,6 @@ param set-default CBRK_AIRSPD_CHK 162128 # 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 -param set-default RBCLW_BAUD 8 -param set-default RBCLW_COUNTS_REV 1200 -param set-default RBCLW_ADDRESS 128 -# 104 corresponds to Telem 4 -param set-default RBCLW_SER_CFG 104 -# Start this driver after setting parameters, because the driver uses some of those parameters. -# roboclaw start /dev/ttyS3 - # Set geometry & output configration param set-default CA_AIRFRAME 6 param set-default CA_R_REV 3 diff --git a/src/drivers/roboclaw/CMakeLists.txt b/src/drivers/roboclaw/CMakeLists.txt deleted file mode 100644 index 5543f89146..0000000000 --- a/src/drivers/roboclaw/CMakeLists.txt +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ -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 deleted file mode 100644 index 9315e757bc..0000000000 --- a/src/drivers/roboclaw/Kconfig +++ /dev/null @@ -1,5 +0,0 @@ -menuconfig DRIVERS_ROBOCLAW - bool "roboclaw" - default n - ---help--- - Enable support for roboclaw \ No newline at end of file diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp deleted file mode 100644 index 89c2ee783e..0000000000 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ /dev/null @@ -1,610 +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.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 deleted file mode 100644 index 0a92d07f39..0000000000 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ /dev/null @@ -1,245 +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 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 deleted file mode 100644 index 81e5f694df..0000000000 --- a/src/drivers/roboclaw/module.yaml +++ /dev/null @@ -1,6 +0,0 @@ -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 deleted file mode 100644 index 66f5d03101..0000000000 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ /dev/null @@ -1,207 +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](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 the 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 deleted file mode 100644 index 3f3c938c55..0000000000 --- a/src/drivers/roboclaw/roboclaw_params.c +++ /dev/null @@ -1,114 +0,0 @@ -/**************************************************************************** - * - * 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);