dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Roboclaw: Integrated OutputModuleInterface including a large code refactor

This commit is contained in:
PerFrivik 2023-11-10 10:43:44 +01:00 committed by Matthias Grob
parent 86e5561a64
commit a40120c332
2 changed files with 351 additions and 547 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

693
src/drivers/roboclaw/RoboClaw.cpp
View File

@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -41,6 +41,7 @@
* *
*/ */
#include "RoboClaw.hpp" #include "RoboClaw.hpp"
#include <poll.h> #include <poll.h>
#include <stdio.h> #include <stdio.h>
@ -74,321 +75,235 @@
// Number of bytes for commands 18 and 19, read speeds. // Number of bytes for commands 18 and 19, read speeds.
#define ENCODER_SPEED_MESSAGE_SIZE 7 #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; 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); strncpy(_storedDeviceName, deviceName, sizeof(_storedDeviceName) - 1);
_storedDeviceName[sizeof(_storedDeviceName) - 1] = '\0'; // Ensure null-termination _storedDeviceName[sizeof(_storedDeviceName) - 1] = '\0'; // Ensure null-termination
strncpy(_storedBaudRateParam, baudRateParam, sizeof(_storedBaudRateParam) - 1); strncpy(_storedBaudRateParam, baudRateParam, sizeof(_storedBaudRateParam) - 1);
_storedBaudRateParam[sizeof(_storedBaudRateParam) - 1] = '\0'; // Ensure null-termination _storedBaudRateParam[sizeof(_storedBaudRateParam) - 1] = '\0'; // Ensure null-termination
initialize();
} }
RoboClaw::~RoboClaw() RoboClaw::~RoboClaw()
{ {
setMotorDutyCycle(MOTOR_1, 0.0); close(_uart_fd);
setMotorDutyCycle(MOTOR_2, 0.0);
close(_uart);
} }
void int RoboClaw::init() {
RoboClaw::initialize() { _uart_fd_timeout = { .tv_sec = 0, .tv_usec = TIMEOUT_US };
_uart = 0; int32_t baud_rate_parameter_value{0};
_uart_timeout = { .tv_sec = 0, .tv_usec = TIMEOUT_US }; int32_t baud_rate_posix{0};
_actuatorsSub = -1; param_get(param_find(_storedBaudRateParam), &baud_rate_parameter_value);
_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"); switch (baud_rate_parameter_value) {
_param_handles.encoder_read_period_ms = param_find("RBCLW_READ_PER"); case 0: // Auto
_param_handles.counts_per_rev = param_find("RBCLW_COUNTS_REV"); default:
printf("Baudrate parameter: %s\n", _storedBaudRateParam); PX4_ERR("Please configure the port's baud_rate_parameter_value");
_param_handles.serial_baud_rate = param_find(_storedBaudRateParam); break;
_param_handles.address = param_find("RBCLW_ADDRESS");
_parameters_update(); case 2400:
baud_rate_posix = B2400;
break;
PX4_ERR("trying to open uart"); case 9600:
baud_rate_posix = B9600;
break;
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;
case 230400:
baud_rate_posix = B230400;
break;
case 460800:
baud_rate_posix = B460800;
break;
}
// start serial port // start serial port
_uart = open(_storedDeviceName, O_RDWR | O_NOCTTY); _uart_fd = 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_fd < 0) { err(1, "could not open %s", _storedDeviceName); }
int ret = 0; int ret = 0;
struct termios uart_config {}; struct termios uart_config {};
ret = tcgetattr(_uart, &uart_config); ret = tcgetattr(_uart_fd, &uart_config);
if (ret < 0) { err(1, "failed to get attr"); } if (ret < 0) { err(1, "failed to get attr"); }
uart_config.c_oflag &= ~ONLCR; // no CR for every LF uart_config.c_oflag &= ~ONLCR; // no CR for every LF
uart_config.c_cflag &= ~CRTSCTS; 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"); } 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"); } 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"); } if (ret < 0) { err(1, "failed to set attr"); }
FD_ZERO(&_uart_set); FD_ZERO(&_uart_fd_set);
FD_SET(_uart, &_uart_set); FD_SET(_uart_fd, &_uart_fd_set);
// setup default settings, reset encoders
resetEncoders();
}
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. // 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]; uint8_t rbuff[6]; // Number of bytes for command 90 status message, read reads status of the roboclaw.
int err_code = _transaction(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true); 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."); PX4_ERR("Shutting down Roboclaw driver.");
return; return PX4_ERROR;
} else { } else {
PX4_INFO("Successfully connected"); 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");
} }
// This main loop performs two different tasks, asynchronously: delete instance;
// - Send actuator_controls_0 to the Roboclaw as soon as they are available _object.store(nullptr);
// - Read the encoder values at a constant rate _task_id = -1;
// 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"); printf("Ending task_spawn");
int waitTime = 100_ms; return PX4_ERROR;
}
uint64_t encoderTaskLastRun = 0; 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;
uint64_t actuatorsLastWritten = 0; 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;
}
_actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0)); void RoboClaw::Run()
orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms); {
if (should_exit()) {
_armedSub = orb_subscribe(ORB_ID(actuator_armed)); ScheduleClear();
_paramSub = orb_subscribe(ORB_ID(parameter_update)); exit_and_cleanup();
_mixing_output.unregister();
pollfd fds[3]; return;
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 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) ) { //temporary
orb_copy(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuatorControls);
orb_copy(ORB_ID(actuator_armed), _armedSub, &_actuatorArmed);
const bool disarmed = !_actuatorArmed.armed || _actuatorArmed.lockdown || _actuatorArmed.manual_lockdown
|| _actuatorArmed.force_failsafe;
if (disarmed) {
// printf("i am disarmed \n");
setMotorSpeed(MOTOR_1, 0.f);
setMotorSpeed(MOTOR_2, 0.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);
}
actuatorsLastWritten = hrt_absolute_time();
} else {
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");
}
}
} }
_mixing_output.update();
if(!_initialized){
init();
_initialized = true;
}
// check for parameter updates
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
updateParams();
}
_actuator_armed_sub.update();
_mixing_output.updateSubscriptions(false);
if (readEncoder() < 0) {
PX4_ERR("Error reading encoders");
}
orb_unsubscribe(_actuatorsSub);
orb_unsubscribe(_armedSub);
orb_unsubscribe(_paramSub);
} }
int RoboClaw::readEncoder() 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]; if (receiveTransaction(CMD_READ_BOTH_ENCODERS, buffer_positon, ENCODER_MESSAGE_SIZE) < ENCODER_MESSAGE_SIZE) {
// I am saving space by overlapping the two separate motor speeds, so that the final buffer will look like:
// [<speed 1> <speed 2> <status 2> <checksum 2>]
// 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 (!success) {
return -1; return -1;
} }
uint32_t count; if (receiveTransaction(CMD_READ_SPEED_1, buffer_speed_right, ENCODER_SPEED_MESSAGE_SIZE) < ENCODER_SPEED_MESSAGE_SIZE) {
uint32_t speed; return -1;
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;
}
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]);
wheel_encoders_s wheel_encoders{};
wheel_encoders.wheel_angle[0] = static_cast<float>(position_right) / _param_rbclw_counts_rev.get() * M_TWOPI_F;
wheel_encoders.wheel_angle[1] = static_cast<float>(position_left) / _param_rbclw_counts_rev.get() * M_TWOPI_F;
wheel_encoders.wheel_speed[0] = static_cast<float>(speed_right) / _param_rbclw_counts_rev.get() * M_TWOPI_F;
wheel_encoders.wheel_speed[1] = static_cast<float>(speed_left) / _param_rbclw_counts_rev.get() * M_TWOPI_F;
wheel_encoders.timestamp = hrt_absolute_time();
_wheel_encoders_pub.publish(wheel_encoders);
return 1; return 1;
} }
void RoboClaw::printStatus() int32_t RoboClaw::reverseInt32(uint8_t *buffer)
{ {
PX4_ERR("pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n", return (buffer[0] << 24)
double(getMotorPosition(MOTOR_1)), | (buffer[1] << 16)
double(getMotorSpeed(MOTOR_1)), | (buffer[2] << 8)
double(getMotorPosition(MOTOR_2)), | buffer[3];
double(getMotorSpeed(MOTOR_2)));
} }
float RoboClaw::getMotorPosition(e_motor motor) void RoboClaw::setMotorSpeed(e_motor motor, float value)
{
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; e_command command;
@ -410,15 +325,14 @@ int RoboClaw::setMotorSpeed(e_motor motor, float value)
} }
} else { } 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; e_command command;
// send command // send command
@ -429,56 +343,48 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
command = CMD_SIGNED_DUTYCYCLE_2; command = CMD_SIGNED_DUTYCYCLE_2;
} else { } else {
return -1; return;
} }
return _sendSigned16Bit(command, value); 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; 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; 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); data = fabs(data);
if (data > 1.0f) { if (data >= 1.0f) {
data = 1.0f; data = 0.99f;
} }
auto byte = (uint8_t)(data * INT8_MAX); auto byte = (uint8_t)(data * INT8_MAX);
uint8_t recv_byte; sendTransaction(command, &byte, 1);
return _transaction(command, &byte, 1, &recv_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; int16_t duty = math::constrain(data, -1.f, 1.f) * INT16_MAX;
uint8_t buff[2]; uint8_t buff[2];
buff[0] = (duty >> 8) & 0xFF; // High byte buff[0] = (duty >> 8) & 0xFF; // High byte
buff[1] = duty & 0xFF; // Low byte buff[1] = duty & 0xFF; // Low byte
uint8_t recv_buff; sendTransaction(command, (uint8_t *) &buff, 2);
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);
} }
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)
@ -501,190 +407,137 @@ uint16_t RoboClaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init)
return crc; 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) {
// 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 -1; return -1;
} }
err_code = readFromDevice(rbuff, rbytes, recv_checksum, buf); size_t total_bytes_read = 0;
if (err_code != RoboClawError::Success) { while (total_bytes_read < bytes_to_read) {
printError(err_code); 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;
}
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;
}
total_bytes_read += bytes_read;
}
if (total_bytes_read < 2) {
PX4_ERR("Too short payload received\n");
return -1; return -1;
} }
return 1; // Verify checksum
uint8_t address = static_cast<uint8_t>(_param_rbclw_address.get());
uint8_t command = static_cast<uint8_t>(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 (checksum_calc != checksum_recv) {
PX4_ERR("Checksum mismatch\n");
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 (select_status <= 0) {
PX4_ERR("ACK timeout");
if (wbuff) { return;
memcpy(&buf[2], wbuff, wbytes);
} }
// Compute and append checksum if necessary. uint8_t acknowledgement{0};
wbytes = wbytes + (send_checksum ? 4 : 2); int bytes_read = read(_uart_fd, &acknowledgement, 1);
if (send_checksum) { if ((bytes_read != 1) || (acknowledgement != 0xFF)) {
uint16_t sum = _calcCRC(buf, wbytes - 2); PX4_ERR("ACK wrong");
buf[wbytes - 2] = (sum >> 8) & 0xFF; return;
buf[wbytes - 1] = sum & 0xFFu;
} }
}
// Write data to the device. RoboClaw::RoboClawError RoboClaw::writeCommand(e_command cmd)
{
uint8_t buffer[2];
int count = write(_uart, buf, wbytes); buffer[0] = (uint8_t)_param_rbclw_address.get();
buffer[1] = cmd;
// Error checking for the write operation. size_t count = write(_uart_fd, buffer, 2);
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::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; // Add address + command ID
size_t bytes_read = 0; buffer[0] = (uint8_t) _param_rbclw_address.get();
buffer[1] = cmd;
while (bytes_read < rbytes) { // Add payload
if (bytes_to_write > 0 && wbuff) {
_uart_timeout.tv_sec = 0; memcpy(&buffer[2], wbuff, bytes_to_write);
_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;
}
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) { // 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) { // Write to device
return RoboClawError::ChecksumError; // or an equivalent error code of your choosing size_t count = write(_uart_fd, buffer, packet_size);
}
uint16_t checksum_calc = _calcCRC(buf, 2); // assuming buf contains address and command // Not all bytes sent
checksum_calc = _calcCRC(rbuff, bytes_read - 2, checksum_calc); if (count < packet_size) {
uint16_t checksum_recv = (rbuff[bytes_read - 2] << 8) + rbuff[bytes_read - 1]; PX4_ERR("Only wrote %d out of %d bytes", count, bytes_to_write);
return RoboClawError::WriteError;
if (checksum_calc != checksum_recv) {
return RoboClawError::ChecksumError;
}
} }
return RoboClawError::Success;
return RoboClawError::Success; // Return success if everything went well
} }
void RoboClaw::printError(RoboClawError error) int RoboClaw::custom_command(int argc, char *argv[])
{ {
return 0;
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");
}
} }
int RoboClaw::print_usage(const char *reason)
void RoboClaw::_parameters_update()
{ {
param_get(_param_handles.counts_per_rev, &_parameters.counts_per_rev); return 0;
param_get(_param_handles.encoder_read_period_ms, &_parameters.encoder_read_period_ms); }
param_get(_param_handles.actuator_write_period_ms, &_parameters.actuator_write_period_ms);
param_get(_param_handles.address, &_parameters.address); int RoboClaw::print_status()
{
if (_actuatorsSub > 0) { PX4_ERR("Running, Initialized: %f", (double)_initialized);
orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms); return 0;
} }
int32_t baudRate; extern "C" __EXPORT int roboclaw_main(int argc, char *argv[])
param_get(_param_handles.serial_baud_rate, &baudRate); {
return RoboClaw::main(argc, argv);
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;
}
} }

205
src/drivers/roboclaw/RoboClaw.hpp
View File

@ -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 * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
@ -53,25 +53,45 @@
#include <pthread.h> #include <pthread.h>
#include <unistd.h> #include <unistd.h>
#include <uORB/Subscription.hpp>
#include <uORB/PublicationMulti.hpp> #include <uORB/PublicationMulti.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/wheel_encoders.h> #include <uORB/topics/wheel_encoders.h>
#include <uORB/topics/actuator_armed.h> #include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_thrust_setpoint.h> #include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_torque_setpoint.h>
#include <uORB/Subscription.hpp> #include <uORB/topics/differential_drive_control.h>
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/mixer_module/mixer_module.hpp>
/** /**
* This is a driver for the RoboClaw motor controller * This is a driver for the RoboClaw motor controller
*/ */
class RoboClaw class RoboClaw : public ModuleBase<RoboClaw>, public OutputModuleInterface
{ {
public: 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 */ /** control channels */
enum e_channel { enum e_channel {
@ -96,77 +116,68 @@ public:
ReadTimeout ReadTimeout
}; };
/** /** @see ModuleBase */
* constructor static int task_spawn(int argc, char *argv[]);
* @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 */
* deconstructor static int custom_command(int argc, char *argv[]);
*/
virtual ~RoboClaw();
void initialize(); /** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
/** /** @see ModuleBase::print_status() */
* @return position of a motor, rev int print_status() override;
*/
float getMotorPosition(e_motor motor);
/** void Run() override;
* @return speed of a motor, rev/sec
*/ int init();
float getMotorSpeed(e_motor motor);
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 * 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 * 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 * Drive both motors. +1 = full forward, -1 = full backward
*/ */
int drive(float value); void drive(float value);
/** /**
* Turn. +1 = full right, -1 = full left * Turn. +1 = full right, -1 = full left
*/ */
int turn(float value); void turn(float value);
/** /**
* reset the encoders * reset the encoders
* @return status * @return status
*/ */
int resetEncoders(); void resetEncoders();
/** /**
* read data from serial * read data from serial
*/ */
int readEncoder(); int readEncoder();
/**
* print status
*/
void printStatus();
private: 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 _storedDeviceName[256]; // Adjust size as necessary
char _storedBaudRateParam[256]; // Adjust size as necessary char _storedBaudRateParam[256]; // Adjust size as necessary
int _timeout_counter = 0; int _timeout_counter = 0;
bool _successfully_connected{false};
// commands // commands
// We just list the commands we want from the manual here. // We just list the commands we want from the manual here.
enum e_command { enum e_command {
@ -200,99 +211,39 @@ private:
CMD_READ_STATUS = 90 CMD_READ_STATUS = 90
}; };
int _uart_fd{0};
fd_set _uart_fd_set;
struct timeval _uart_fd_timeout;
struct { uORB::SubscriptionData<actuator_armed_s> _actuator_armed_sub{ORB_ID(actuator_armed)};
speed_t serial_baud_rate; uORB::Subscription _differential_drive_control_sub{ORB_ID(differential_drive_control)};
int32_t counts_per_rev; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
int32_t encoder_read_period_ms;
int32_t actuator_write_period_ms;
int32_t address;
} _parameters{};
struct { differential_drive_control_s _diff_drive_control{};
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; matrix::Vector2f _motor_control{0.0f, 0.0f};
fd_set _uart_set;
struct timeval _uart_timeout;
/** actuator controls subscription */ uORB::Publication<wheel_encoders_s> _wheel_encoders_pub {ORB_ID(wheel_encoders)};
int _actuatorsSub{-1};
actuator_controls_s _actuatorControls;
int _armedSub{-1};
actuator_armed_s _actuatorArmed;
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<wheel_encoders_s> _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(); void _parameters_update();
void vehicle_control_poll();
static uint16_t _calcCRC(const uint8_t *buf, size_t n, uint16_t init = 0); static uint16_t _calcCRC(const uint8_t *buf, size_t n, uint16_t init = 0);
int _sendUnsigned7Bit(e_command command, float data); void _sendUnsigned7Bit(e_command command, float data);
int _sendSigned16Bit(e_command command, float data); void _sendSigned16Bit(e_command command, float data);
int _sendNothing(e_command);
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(
* print status (ParamInt<px4::params::RBCLW_ADDRESS>) _param_rbclw_address,
*/ (ParamInt<px4::params::RBCLW_COUNTS_REV>) _param_rbclw_counts_rev
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.
*
* 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);
}; };