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

Roboclaw: major cleanup

This commit is contained in:
Matthias Grob 2023-11-14 16:37:09 +01:00
parent c27181a154
commit f53edfa440
3 changed files with 279 additions and 392 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
msg/WheelEncoders.msg
View File

@ -1,4 +1,5 @@
uint64 timestamp # time since system start (microseconds) 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. # Two wheels: 0 left, 1 right
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. float32[2] wheel_speed # [rad/s]
float32[2] wheel_angle # [rad]

479
src/drivers/roboclaw/Roboclaw.cpp
View File

@ -41,50 +41,17 @@
* *
*/ */
#include "Roboclaw.hpp" #include "Roboclaw.hpp"
#include <poll.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <fcntl.h>
#include <termios.h> #include <termios.h>
#include <systemlib/err.h> Roboclaw::Roboclaw(const char *device_name, const char *bad_rate_parameter) :
#include <systemlib/mavlink_log.h>
#include <uORB/Publication.hpp>
#include <drivers/drv_hrt.h>
#include <math.h>
#include <mathlib/mathlib.h>
// 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) :
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default) OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default)
{ {
strncpy(_storedDeviceName, deviceName, sizeof(_storedDeviceName) - 1); strncpy(_stored_device_name, device_name, sizeof(_stored_device_name) - 1);
_storedDeviceName[sizeof(_storedDeviceName) - 1] = '\0'; // Ensure null-termination _stored_device_name[sizeof(_stored_device_name) - 1] = '\0'; // Ensure null-termination
strncpy(_storedBaudRateParam, baudRateParam, sizeof(_storedBaudRateParam) - 1); strncpy(_stored_baud_rate_parameter, bad_rate_parameter, sizeof(_stored_baud_rate_parameter) - 1);
_storedBaudRateParam[sizeof(_storedBaudRateParam) - 1] = '\0'; // Ensure null-termination _stored_baud_rate_parameter[sizeof(_stored_baud_rate_parameter) - 1] = '\0'; // Ensure null-termination
} }
Roboclaw::~Roboclaw() Roboclaw::~Roboclaw()
@ -92,13 +59,16 @@ Roboclaw::~Roboclaw()
close(_uart_fd); 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 }; _uart_fd_timeout = { .tv_sec = 0, .tv_usec = TIMEOUT_US };
int32_t baud_rate_parameter_value{0}; int32_t baud_rate_parameter_value{0};
int32_t baud_rate_posix{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) { switch (baud_rate_parameter_value) {
case 0: // Auto case 0: // Auto
@ -140,9 +110,9 @@ int Roboclaw::init()
} }
// start serial port // 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; int ret = 0;
struct termios uart_config {}; struct termios uart_config {};
@ -169,49 +139,21 @@ int Roboclaw::init()
FD_ZERO(&_uart_fd_set); FD_ZERO(&_uart_fd_set);
FD_SET(_uart_fd, &_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. // Make sure the device does respond
uint8_t rbuff[6]; // Number of bytes for command 90 status message, read reads status of the roboclaw. static constexpr int READ_STATUS_RESPONSE_SIZE = 6;
int err_code = receiveTransaction(CMD_READ_STATUS, &rbuff[0], sizeof(rbuff)); uint8_t response_buffer[READ_STATUS_RESPONSE_SIZE];
if (err_code <= 0) { if (receiveTransaction(Command::ReadStatus, response_buffer, READ_STATUS_RESPONSE_SIZE) < READ_STATUS_RESPONSE_SIZE) {
PX4_ERR("Shutting down Roboclaw driver."); PX4_ERR("No valid response, stopping driver");
return PX4_ERROR; request_stop();
return ERROR;
} else { } else {
PX4_INFO("Successfully connected"); PX4_INFO("Successfully connected");
/* Schedule a cycle to start things. */ return OK;
_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");
}
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], 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)
{ {
@ -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; float right_motor_output = ((float)outputs[0] - 128.0f) / 127.f;
if (stop_motors) { if (stop_motors) {
setMotorSpeed(MOTOR_1, 0.f); setMotorSpeed(Motor::Left, 0.f);
setMotorSpeed(MOTOR_2, 0.f); setMotorSpeed(Motor::Right, 0.f);
} else { } else {
// PX4_ERR("Left and Right motor speed %f %f", (double)left_motor_output, (double)right_motor_output); setMotorSpeed(Motor::Left, left_motor_output);
setMotorSpeed(MOTOR_1, left_motor_output); setMotorSpeed(Motor::Right, right_motor_output);
setMotorSpeed(MOTOR_2, right_motor_output);
} }
return true; return true;
@ -242,9 +183,9 @@ void Roboclaw::Run()
_mixing_output.update(); _mixing_output.update();
if (!_initialized) { if (!_uart_initialized) {
init(); initializeUART();
_initialized = true; _uart_initialized = true;
} }
// check for parameter updates // check for parameter updates
@ -257,121 +198,104 @@ void Roboclaw::Run()
} }
_actuator_armed_sub.update(); _actuator_armed_sub.update();
_mixing_output.updateSubscriptions(false); _mixing_output.updateSubscriptions(false);
if (readEncoder() < 0) { if (readEncoder() != OK) {
PX4_ERR("Error reading encoders"); PX4_ERR("Error reading encoders");
} }
} }
int Roboclaw::readEncoder() 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_positon[ENCODER_MESSAGE_SIZE];
uint8_t buffer_speed_right[ENCODER_SPEED_MESSAGE_SIZE]; uint8_t buffer_speed_right[ENCODER_SPEED_MESSAGE_SIZE];
uint8_t buffer_speed_left[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) { if (receiveTransaction(Command::ReadSpeedMotor2, buffer_speed_left,
return -1; 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) { if (receiveTransaction(Command::ReadSpeedMotor1, buffer_speed_right,
return -1; 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) { if (receiveTransaction(Command::ReadEncoderCounters, buffer_positon, ENCODER_MESSAGE_SIZE) < ENCODER_MESSAGE_SIZE) {
return -1; return ERROR;
} }
int32_t position_right = reverseInt32(&buffer_positon[0]); int32_t speed_left = swapBytesInt32(&buffer_speed_left[0]);
int32_t position_left = reverseInt32(&buffer_positon[4]); int32_t speed_right = swapBytesInt32(&buffer_speed_right[0]);
int32_t speed_right = reverseInt32(&buffer_speed_right[0]); int32_t position_left = swapBytesInt32(&buffer_positon[4]);
int32_t speed_left = reverseInt32(&buffer_speed_left[0]); int32_t position_right = swapBytesInt32(&buffer_positon[0]);
wheel_encoders_s wheel_encoders{}; 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_speed[0] = static_cast<float>(speed_left) / _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[1] = static_cast<float>(speed_right) / _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_angle[0] = static_cast<float>(position_left) / _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.wheel_angle[1] = static_cast<float>(position_right) / _param_rbclw_counts_rev.get() * M_TWOPI_F;
wheel_encoders.timestamp = hrt_absolute_time(); wheel_encoders.timestamp = hrt_absolute_time();
_wheel_encoders_pub.publish(wheel_encoders); _wheel_encoders_pub.publish(wheel_encoders);
return 1; return OK;
} }
int32_t Roboclaw::reverseInt32(uint8_t *buffer) void Roboclaw::setMotorSpeed(Motor motor, float value)
{ {
return (buffer[0] << 24) Command command;
| (buffer[1] << 16)
| (buffer[2] << 8)
| buffer[3];
}
void Roboclaw::setMotorSpeed(e_motor motor, float value)
{
e_command command;
// send command // send command
if (motor == MOTOR_1) { if (motor == Motor::Left) {
if (value > 0) { if (value > 0) {
command = CMD_DRIVE_FWD_1; command = Command::DriveForwardMotor1;
} else { } else {
command = CMD_DRIVE_REV_1; command = Command::DriveBackwardsMotor1;
} }
} else if (motor == MOTOR_2) { } else if (motor == Motor::Right) {
if (value > 0) { if (value > 0) {
command = CMD_DRIVE_FWD_2; command = Command::DriveForwardMotor2;
} else { } else {
command = CMD_DRIVE_REV_2; command = Command::DriveBackwardsMotor2;
} }
} else { } else {
return; 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 // send command
if (motor == MOTOR_1) { if (motor == Motor::Left) {
command = CMD_SIGNED_DUTYCYCLE_1; command = Command::DutyCycleMotor1;
} else if (motor == MOTOR_2) { } else if (motor == Motor::Right) {
command = CMD_SIGNED_DUTYCYCLE_2; command = Command::DutyCycleMotor2;
} else { } else {
return; return;
} }
return _sendSigned16Bit(command, value); 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);
} }
void Roboclaw::resetEncoders() 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); data = fabs(data);
@ -383,21 +307,151 @@ void Roboclaw::_sendUnsigned7Bit(e_command command, float data)
sendTransaction(command, &byte, 1); 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]; uint8_t buff[2];
buff[0] = (duty >> 8) & 0xFF; // High byte buff[0] = (value >> 8) & 0xFF; // High byte
buff[1] = duty & 0xFF; // Low byte buff[1] = value & 0xFF; // Low byte
sendTransaction(command, (uint8_t *) &buff, 2); 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)
{
if (writeCommandWithPayload(cmd, write_buffer, bytes_to_write) != OK) {
return ERROR;
}
return readAcknowledgement();
}
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];
// Add address + command ID
buffer[0] = (uint8_t) _param_rbclw_address.get();
buffer[1] = static_cast<uint8_t>(command);
// Add payload
if (bytes_to_write > 0 && wbuff) {
memcpy(&buffer[2], wbuff, bytes_to_write);
}
// 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::readAcknowledgement()
{
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<uint8_t>(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) {
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 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 ERROR;
}
total_bytes_read += bytes_read;
}
if (total_bytes_read < 2) {
PX4_ERR("Too short payload received\n");
return ERROR;
}
// Verify response checksum
uint8_t address = static_cast<uint8_t>(_param_rbclw_address.get());
uint8_t command_byte = static_cast<uint8_t>(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 (crc_calculated != crc_received) {
PX4_ERR("Checksum mismatch\n");
return ERROR;
}
return total_bytes_read;
}
uint16_t Roboclaw::_calcCRC(const uint8_t *buffer, size_t bytes, uint16_t init)
{ {
uint16_t crc = init; uint16_t crc = init;
for (size_t byte = 0; byte < n; byte++) { for (size_t byte = 0; byte < bytes; byte++) {
crc = crc ^ (((uint16_t) buf[byte]) << 8); crc = crc ^ (((uint16_t) buffer[byte]) << 8);
for (uint8_t bit = 0; bit < 8; bit++) { for (uint8_t bit = 0; bit < 8; bit++) {
if (crc & 0x8000) { if (crc & 0x8000) {
@ -412,118 +466,38 @@ uint16_t Roboclaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init)
return crc; return crc;
} }
int Roboclaw::receiveTransaction(e_command cmd, uint8_t *read_buffer, size_t bytes_to_read) int32_t Roboclaw::swapBytesInt32(uint8_t *buffer)
{ {
if (writeCommand(cmd) != RoboClawError::Success) { return (buffer[0] << 24)
return -1; | (buffer[1] << 16)
} | (buffer[2] << 8)
| buffer[3];
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;
}
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;
}
// 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;
} }
void Roboclaw::sendTransaction(e_command cmd, uint8_t *write_buffer, size_t bytes_to_write) int Roboclaw::task_spawn(int argc, char *argv[])
{ {
writeCommandWithPayload(cmd, write_buffer, bytes_to_write); const char *device_name = argv[1];
int select_status = select(_uart_fd + 1, &_uart_fd_set, nullptr, nullptr, &_uart_fd_timeout); const char *baud_rate_parameter_value = argv[2];
if (select_status <= 0) { Roboclaw *instance = new Roboclaw(device_name, baud_rate_parameter_value);
PX4_ERR("ACK timeout");
return; if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
instance->ScheduleNow();
return OK;
} else {
PX4_ERR("alloc failed");
} }
uint8_t acknowledgement{0}; delete instance;
int bytes_read = read(_uart_fd, &acknowledgement, 1); _object.store(nullptr);
_task_id = -1;
if ((bytes_read != 1) || (acknowledgement != 0xFF)) { printf("Ending task_spawn");
PX4_ERR("ACK wrong");
return;
}
}
Roboclaw::RoboClawError Roboclaw::writeCommand(e_command cmd) return ERROR;
{
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;
}
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];
// Add address + command ID
buffer[0] = (uint8_t) _param_rbclw_address.get();
buffer[1] = cmd;
// Add payload
if (bytes_to_write > 0 && wbuff) {
memcpy(&buffer[2], wbuff, bytes_to_write);
}
// 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 count = write(_uart_fd, buffer, packet_size);
// 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;
} }
int Roboclaw::custom_command(int argc, char *argv[]) int Roboclaw::custom_command(int argc, char *argv[])
@ -562,7 +536,6 @@ The command to start this driver is: `$ roboclaw start <UART device> <baud rate>
int Roboclaw::print_status() int Roboclaw::print_status()
{ {
PX4_ERR("Running, Initialized: %f", (double)_initialized);
return 0; return 0;
} }

187
src/drivers/roboclaw/Roboclaw.hpp
View File

@ -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 * Product page: https://www.basicmicro.com/motor-controller
* Manual: https://downloads.basicmicro.com/docs/roboclaw_user_manual.pdf * Manual: https://downloads.basicmicro.com/docs/roboclaw_user_manual.pdf
@ -54,172 +54,85 @@
#include <uORB/Publication.hpp> #include <uORB/Publication.hpp>
#include <uORB/topics/wheel_encoders.h> #include <uORB/topics/wheel_encoders.h>
/**
* This is a driver for the Roboclaw motor controller
*/
class Roboclaw : public ModuleBase<Roboclaw>, public OutputModuleInterface class Roboclaw : public ModuleBase<Roboclaw>, public OutputModuleInterface
{ {
public: public:
/** /**
* constructor * @param device_name Name of the serial port e.g. "/dev/ttyS2"
* @param deviceName the name of the * @param bad_rate_parameter Name of the parameter that holds the baud rate of this serial port
* 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
*/ */
Roboclaw(const char *device_name, const char *bad_rate_parameter);
virtual ~Roboclaw(); virtual ~Roboclaw();
/** control channels */ enum class Motor {
enum e_channel { Left = 0,
CH_VOLTAGE_LEFT = 0, Right = 1
CH_VOLTAGE_RIGHT
}; };
/** motors */ static int task_spawn(int argc, char *argv[]); ///< @see ModuleBase
enum e_motor { static int custom_command(int argc, char *argv[]); ///< @see ModuleBase
MOTOR_1 = 0, static int print_usage(const char *reason = nullptr); ///< @see ModuleBase
MOTOR_2 int print_status() override; ///< @see ModuleBase
};
/** 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;
void Run() override; void Run() override;
int init(); /** @see OutputModuleInterface */
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], 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;
/** void setMotorSpeed(Motor motor, float value); ///< rev/sec
* set the speed of a motor, rev/sec void setMotorDutyCycle(Motor motor, float value);
*/ int readEncoder();
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(); void resetEncoders();
/**
* read data from serial
*/
int readEncoder();
private: private:
static constexpr int MAX_ACTUATORS = 2; enum class Command : uint8_t {
ReadStatus = 90,
MixingOutput _mixing_output{"ROBOCLAW", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false}; DriveForwardMotor1 = 0,
DriveBackwardsMotor1 = 1,
DriveForwardMotor2 = 4,
DriveBackwardsMotor2 = 5,
DutyCycleMotor1 = 32,
DutyCycleMotor2 = 33,
char _storedDeviceName[256]; // Adjust size as necessary ReadSpeedMotor1 = 18,
char _storedBaudRateParam[256]; // Adjust size as necessary ReadSpeedMotor2 = 19,
ResetEncoders = 20,
int _timeout_counter = 0; ReadEncoderCounters = 78,
bool _successfully_connected{false};
// 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
}; };
int _uart_fd{0}; static constexpr int MAX_ACTUATORS = 2;
fd_set _uart_fd_set; MixingOutput _mixing_output{"ROBOCLAW", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false};
struct timeval _uart_fd_timeout;
uORB::SubscriptionData<actuator_armed_s> _actuator_armed_sub{ORB_ID(actuator_armed)}; uORB::SubscriptionData<actuator_armed_s> _actuator_armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Publication<wheel_encoders_s> _wheel_encoders_pub{ORB_ID(wheel_encoders)};
matrix::Vector2f _motor_control{0.0f, 0.0f}; char _stored_device_name[256]; // Adjust size as necessary
char _stored_baud_rate_parameter[256]; // Adjust size as necessary
uORB::Publication<wheel_encoders_s> _wheel_encoders_pub {ORB_ID(wheel_encoders)}; void sendUnsigned7Bit(Command command, float data);
void sendSigned16Bit(Command command, float data);
void _parameters_update(); // 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();
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);
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);
void _sendUnsigned7Bit(e_command command, float data); int32_t swapBytesInt32(uint8_t *buffer);
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);
// UART handling
int initializeUART();
bool _uart_initialized{false};
int _uart_fd{0};
fd_set _uart_fd_set;
struct timeval _uart_fd_timeout;
DEFINE_PARAMETERS( DEFINE_PARAMETERS(
(ParamInt<px4::params::RBCLW_ADDRESS>) _param_rbclw_address, (ParamInt<px4::params::RBCLW_ADDRESS>) _param_rbclw_address,