Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Roboclaw ESC driver #22215

Merged
merged 27 commits into from
Nov 28, 2023
Merged
Changes from 1 commit
Commits
Show all changes
27 commits
Select commit Hold shift + click to select a range
f5489b0
ported roboclaw driver from older commits into newest develop branch
PerFrivik Oct 4, 2023
3bf9122
Roboclaw: Temporary fix, enabling driver to run
PerFrivik Oct 9, 2023
29c1270
Driving possible
MaEtUgR Dec 14, 2021
3aefa72
Roboclaw: Added DutyCycle command in Roboclaw destructor to turn off …
PerFrivik Oct 10, 2023
f09878f
Roboclaw: Working temporary version that drives around
PerFrivik Oct 10, 2023
7682456
Roboclaw: Replaced setDutyCycle with setMotorSpeed to allow for encod…
PerFrivik Oct 11, 2023
2dc976e
Roboclaw: Initial cleanup, next commit will be refactor removing the …
PerFrivik Oct 11, 2023
18416e8
Roboclaw: rough refactor, removed repetitive code, simplified and cla…
PerFrivik Oct 11, 2023
8d39d63
Roboclaw: Fixed issue when power cylcing the roboclaw where the drive…
PerFrivik Oct 12, 2023
51ed2aa
Roboclaw: Integrated OutputModuleInterface including a large code ref…
PerFrivik Nov 10, 2023
4edc4ed
Update msg/CMakeLists.txt
PerFrivik Nov 10, 2023
5e9ed2d
Delete msg/ActuatorControls.msg
PerFrivik Nov 10, 2023
d9daaa9
Update WheelEncoders.msg
PerFrivik Nov 10, 2023
1136999
Update RoverPositionControl.cpp
PerFrivik Nov 10, 2023
b56fc3a
RoboClaw: fix style
MaEtUgR Nov 13, 2023
18b1590
RoboClaw: declutter, make it compile again
MaEtUgR Nov 13, 2023
b6999c2
Roboclaw: move parameters to module.yaml
MaEtUgR Nov 14, 2023
627244e
Rename RoboClaw -> Roboclaw
MaEtUgR Nov 14, 2023
00ca59c
Roboclaw: major cleanup
MaEtUgR Nov 14, 2023
aa1366d
Roboclaw: Consistent Left & Right naming convertion with Differential…
PerFrivik Nov 15, 2023
74e103b
Roboclaw: Updated Airframe and fixed left and right mapping error
PerFrivik Nov 15, 2023
ea96cc5
Roboclaw: Accidentally removed a parameter
PerFrivik Nov 15, 2023
6939bfd
Roboclaw: Changes in r1 airframe, removed hardcoded port configurations
PerFrivik Nov 20, 2023
ce8123a
Roboclaw: Updated yaml file to support Roboclaw Driver in QGC
PerFrivik Nov 20, 2023
c9bf3e2
Roboclaw: Fix CI pr issue
PerFrivik Nov 28, 2023
2baf4a6
Roboclaw: Fixed issue where parameters had different prefixes
PerFrivik Nov 28, 2023
f06b931
Roboclaw: Updated parameter prefix for roboclaw output module
PerFrivik Nov 28, 2023
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Roboclaw: major cleanup
MaEtUgR authored and PerFrivik committed Nov 20, 2023
commit 00ca59cc3a32c09cf1fe6dc207a1235eeeca5a9a
5 changes: 3 additions & 2 deletions msg/WheelEncoders.msg
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
uint64 timestamp # time since system start (microseconds)

float32[4] wheel_angle #Wheel angle of encoder, if two wheels wheel_angle[0] = right wheel and wheel_angle[1] = left wheel. Else [0][1] = right and [2][3] are left.
float32[4] wheel_speed #Wheel speed of encoder, if two wheels wheel_angle[0] = right wheel and wheel_angle[1] = left wheel. Else [0][1] = right and [2][3] are left.
# Two wheels: 0 left, 1 right
float32[2] wheel_speed # [rad/s]
float32[2] wheel_angle # [rad]
415 changes: 194 additions & 221 deletions src/drivers/roboclaw/Roboclaw.cpp
Original file line number Diff line number Diff line change
@@ -41,64 +41,34 @@
*
*/


#include "Roboclaw.hpp"
#include <poll.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <fcntl.h>
#include <termios.h>

#include <systemlib/err.h>
#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) :
Roboclaw::Roboclaw(const char *device_name, const char *bad_rate_parameter) :
OutputModuleInterface(MODULE_NAME, px4::wq_configurations::hp_default)
{
strncpy(_storedDeviceName, deviceName, sizeof(_storedDeviceName) - 1);
_storedDeviceName[sizeof(_storedDeviceName) - 1] = '\0'; // Ensure null-termination
strncpy(_stored_device_name, device_name, sizeof(_stored_device_name) - 1);
_stored_device_name[sizeof(_stored_device_name) - 1] = '\0'; // Ensure null-termination

strncpy(_storedBaudRateParam, baudRateParam, sizeof(_storedBaudRateParam) - 1);
_storedBaudRateParam[sizeof(_storedBaudRateParam) - 1] = '\0'; // Ensure null-termination
strncpy(_stored_baud_rate_parameter, bad_rate_parameter, sizeof(_stored_baud_rate_parameter) - 1);
_stored_baud_rate_parameter[sizeof(_stored_baud_rate_parameter) - 1] = '\0'; // Ensure null-termination
}

Roboclaw::~Roboclaw()
{
close(_uart_fd);
}

int Roboclaw::init()
int Roboclaw::initializeUART()
{
// The Roboclaw has a serial communication timeout of 10ms
// Add a little extra to account for timing inaccuracy
static constexpr int TIMEOUT_US = 11_ms;
_uart_fd_timeout = { .tv_sec = 0, .tv_usec = TIMEOUT_US };

int32_t baud_rate_parameter_value{0};
int32_t baud_rate_posix{0};
param_get(param_find(_storedBaudRateParam), &baud_rate_parameter_value);
param_get(param_find(_stored_baud_rate_parameter), &baud_rate_parameter_value);

switch (baud_rate_parameter_value) {
case 0: // Auto
@@ -140,9 +110,9 @@ int Roboclaw::init()
}

// start serial port
_uart_fd = open(_storedDeviceName, O_RDWR | O_NOCTTY);
_uart_fd = open(_stored_device_name, O_RDWR | O_NOCTTY);

if (_uart_fd < 0) { err(1, "could not open %s", _storedDeviceName); }
if (_uart_fd < 0) { err(1, "could not open %s", _stored_device_name); }

int ret = 0;
struct termios uart_config {};
@@ -169,47 +139,19 @@ int Roboclaw::init()
FD_ZERO(&_uart_fd_set);
FD_SET(_uart_fd, &_uart_fd_set);

// Make sure the Roboclaw is actually connected, so I don't just spam errors if it's not.
uint8_t rbuff[6]; // Number of bytes for command 90 status message, read reads status of the roboclaw.
int err_code = receiveTransaction(CMD_READ_STATUS, &rbuff[0], sizeof(rbuff));
// Make sure the device does respond
static constexpr int READ_STATUS_RESPONSE_SIZE = 6;
uint8_t response_buffer[READ_STATUS_RESPONSE_SIZE];

if (err_code <= 0) {
PX4_ERR("Shutting down Roboclaw driver.");
return PX4_ERROR;
if (receiveTransaction(Command::ReadStatus, response_buffer, READ_STATUS_RESPONSE_SIZE) < READ_STATUS_RESPONSE_SIZE) {
PX4_ERR("No valid response, stopping driver");
request_stop();
return ERROR;

} else {
PX4_INFO("Successfully connected");
/* Schedule a cycle to start things. */
_successfully_connected = true;
return PX4_OK;
}
}

int Roboclaw::task_spawn(int argc, char *argv[])
{
const char *deviceName = argv[1];
const char *baud_rate_parameter_value = argv[2];

Roboclaw *instance = new Roboclaw(deviceName, baud_rate_parameter_value);

if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
// instance->ScheduleOnInterval(10_ms); // 100 Hz
instance->ScheduleNow();
return PX4_OK;

} else {
PX4_ERR("alloc failed");
return OK;
}

delete instance;
_object.store(nullptr);
_task_id = -1;

printf("Ending task_spawn");

return PX4_ERROR;
}

bool Roboclaw::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
@@ -219,13 +161,12 @@ bool Roboclaw::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
float right_motor_output = ((float)outputs[0] - 128.0f) / 127.f;

if (stop_motors) {
setMotorSpeed(MOTOR_1, 0.f);
setMotorSpeed(MOTOR_2, 0.f);
setMotorSpeed(Motor::Left, 0.f);
setMotorSpeed(Motor::Right, 0.f);

} else {
// PX4_ERR("Left and Right motor speed %f %f", (double)left_motor_output, (double)right_motor_output);
setMotorSpeed(MOTOR_1, left_motor_output);
setMotorSpeed(MOTOR_2, right_motor_output);
setMotorSpeed(Motor::Left, left_motor_output);
setMotorSpeed(Motor::Right, right_motor_output);
}

return true;
@@ -242,9 +183,9 @@ void Roboclaw::Run()

_mixing_output.update();

if (!_initialized) {
init();
_initialized = true;
if (!_uart_initialized) {
initializeUART();
_uart_initialized = true;
}

// check for parameter updates
@@ -257,121 +198,104 @@ void Roboclaw::Run()
}

_actuator_armed_sub.update();

_mixing_output.updateSubscriptions(false);

if (readEncoder() < 0) {
if (readEncoder() != OK) {
PX4_ERR("Error reading encoders");
}

}

int Roboclaw::readEncoder()
{
static constexpr int ENCODER_MESSAGE_SIZE = 10; // response size for ReadEncoderCounters
static constexpr int ENCODER_SPEED_MESSAGE_SIZE = 7; // response size for CMD_READ_SPEED_{1,2}

uint8_t buffer_positon[ENCODER_MESSAGE_SIZE];
uint8_t buffer_speed_right[ENCODER_SPEED_MESSAGE_SIZE];
uint8_t buffer_speed_left[ENCODER_SPEED_MESSAGE_SIZE];

if (receiveTransaction(CMD_READ_BOTH_ENCODERS, buffer_positon, ENCODER_MESSAGE_SIZE) < ENCODER_MESSAGE_SIZE) {
return -1;
if (receiveTransaction(Command::ReadSpeedMotor2, buffer_speed_left,
ENCODER_SPEED_MESSAGE_SIZE) < ENCODER_SPEED_MESSAGE_SIZE) {
return ERROR;
}

if (receiveTransaction(CMD_READ_SPEED_1, buffer_speed_right, ENCODER_SPEED_MESSAGE_SIZE) < ENCODER_SPEED_MESSAGE_SIZE) {
return -1;
if (receiveTransaction(Command::ReadSpeedMotor1, buffer_speed_right,
ENCODER_SPEED_MESSAGE_SIZE) < ENCODER_SPEED_MESSAGE_SIZE) {
return ERROR;
}

if (receiveTransaction(CMD_READ_SPEED_2, buffer_speed_left, ENCODER_SPEED_MESSAGE_SIZE) < ENCODER_SPEED_MESSAGE_SIZE) {
return -1;
if (receiveTransaction(Command::ReadEncoderCounters, buffer_positon, ENCODER_MESSAGE_SIZE) < ENCODER_MESSAGE_SIZE) {
return ERROR;
}

int32_t position_right = reverseInt32(&buffer_positon[0]);
int32_t position_left = reverseInt32(&buffer_positon[4]);
int32_t speed_right = reverseInt32(&buffer_speed_right[0]);
int32_t speed_left = reverseInt32(&buffer_speed_left[0]);
int32_t speed_left = swapBytesInt32(&buffer_speed_left[0]);
int32_t speed_right = swapBytesInt32(&buffer_speed_right[0]);
int32_t position_left = swapBytesInt32(&buffer_positon[4]);
int32_t position_right = swapBytesInt32(&buffer_positon[0]);

wheel_encoders_s wheel_encoders{};
wheel_encoders.wheel_angle[0] = static_cast<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.wheel_speed[0] = static_cast<float>(speed_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_angle[0] = static_cast<float>(position_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_pub.publish(wheel_encoders);

return 1;
}

int32_t Roboclaw::reverseInt32(uint8_t *buffer)
{
return (buffer[0] << 24)
| (buffer[1] << 16)
| (buffer[2] << 8)
| buffer[3];
return OK;
}

void Roboclaw::setMotorSpeed(e_motor motor, float value)
void Roboclaw::setMotorSpeed(Motor motor, float value)
{
e_command command;
Command command;

// send command
if (motor == MOTOR_1) {
if (motor == Motor::Left) {
if (value > 0) {
command = CMD_DRIVE_FWD_1;
command = Command::DriveForwardMotor1;

} else {
command = CMD_DRIVE_REV_1;
command = Command::DriveBackwardsMotor1;
}

} else if (motor == MOTOR_2) {
} else if (motor == Motor::Right) {
if (value > 0) {
command = CMD_DRIVE_FWD_2;
command = Command::DriveForwardMotor2;

} else {
command = CMD_DRIVE_REV_2;
command = Command::DriveBackwardsMotor2;
}

} else {
return;
}

_sendUnsigned7Bit(command, value);
sendUnsigned7Bit(command, value);
}

void Roboclaw::setMotorDutyCycle(e_motor motor, float value)
void Roboclaw::setMotorDutyCycle(Motor motor, float value)
{
e_command command;
Command command;

// send command
if (motor == MOTOR_1) {
command = CMD_SIGNED_DUTYCYCLE_1;
if (motor == Motor::Left) {
command = Command::DutyCycleMotor1;

} else if (motor == MOTOR_2) {
command = CMD_SIGNED_DUTYCYCLE_2;
} else if (motor == Motor::Right) {
command = Command::DutyCycleMotor2;

} else {
return;
}

return _sendSigned16Bit(command, value);
}

void Roboclaw::drive(float value)
{
e_command command = value >= 0 ? CMD_DRIVE_FWD_MIX : CMD_DRIVE_REV_MIX;
_sendUnsigned7Bit(command, value);
}

void Roboclaw::turn(float value)
{
e_command command = value >= 0 ? CMD_TURN_LEFT : CMD_TURN_RIGHT;
_sendUnsigned7Bit(command, value);
return sendSigned16Bit(command, value);
}

void Roboclaw::resetEncoders()
{
sendTransaction(CMD_RESET_ENCODERS, nullptr, 0);
sendTransaction(Command::ResetEncoders, nullptr, 0);
}

void Roboclaw::_sendUnsigned7Bit(e_command command, float data)
void Roboclaw::sendUnsigned7Bit(Command command, float data)
{
data = fabs(data);

@@ -383,147 +307,197 @@ void Roboclaw::_sendUnsigned7Bit(e_command command, float data)
sendTransaction(command, &byte, 1);
}

void Roboclaw::_sendSigned16Bit(e_command command, float data)
void Roboclaw::sendSigned16Bit(Command command, float data)
{
int16_t duty = math::constrain(data, -1.f, 1.f) * INT16_MAX;
int16_t value = math::constrain(data, -1.f, 1.f) * INT16_MAX;
uint8_t buff[2];
buff[0] = (duty >> 8) & 0xFF; // High byte
buff[1] = duty & 0xFF; // Low byte
buff[0] = (value >> 8) & 0xFF; // High byte
buff[1] = value & 0xFF; // Low byte
sendTransaction(command, (uint8_t *) &buff, 2);
}

uint16_t Roboclaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init)
int Roboclaw::sendTransaction(Command cmd, uint8_t *write_buffer, size_t bytes_to_write)
{
uint16_t crc = init;
if (writeCommandWithPayload(cmd, write_buffer, bytes_to_write) != OK) {
return ERROR;
}

for (size_t byte = 0; byte < n; byte++) {
crc = crc ^ (((uint16_t) buf[byte]) << 8);
return readAcknowledgement();
}

for (uint8_t bit = 0; bit < 8; bit++) {
if (crc & 0x8000) {
crc = (crc << 1) ^ 0x1021;
int Roboclaw::writeCommandWithPayload(Command command, uint8_t *wbuff, size_t bytes_to_write)
{
size_t packet_size = 2 + bytes_to_write + 2;
uint8_t buffer[packet_size];

} else {
crc = crc << 1;
}
}
// Add address + command ID
buffer[0] = (uint8_t) _param_rbclw_address.get();
buffer[1] = static_cast<uint8_t>(command);

// Add payload
if (bytes_to_write > 0 && wbuff) {
memcpy(&buffer[2], wbuff, bytes_to_write);
}

return crc;
// Add checksum
uint16_t sum = _calcCRC(buffer, packet_size - 2);
buffer[packet_size - 2] = (sum >> 8) & 0xFF;
buffer[packet_size - 1] = sum & 0xFFu;

// Write to device
size_t bytes_written = write(_uart_fd, buffer, packet_size);

// Not all bytes sent
if (bytes_written < packet_size) {
PX4_ERR("Only wrote %d out of %d bytes", bytes_written, bytes_to_write);
return ERROR;
}

return OK;
}

int Roboclaw::receiveTransaction(e_command cmd, uint8_t *read_buffer, size_t bytes_to_read)
int Roboclaw::readAcknowledgement()
{
if (writeCommand(cmd) != RoboClawError::Success) {
return -1;
int select_status = select(_uart_fd + 1, &_uart_fd_set, nullptr, nullptr, &_uart_fd_timeout);

if (select_status <= 0) {
PX4_ERR("ACK timeout");
return ERROR;
}

uint8_t acknowledgement{0};
int bytes_read = read(_uart_fd, &acknowledgement, 1);

if ((bytes_read != 1) || (acknowledgement != 0xFF)) {
PX4_ERR("ACK wrong");
return ERROR;
}

return OK;
}

int Roboclaw::receiveTransaction(Command command, uint8_t *read_buffer, size_t bytes_to_read)
{
if (writeCommand(command) != OK) {
return ERROR;
}

return readResponse(command, read_buffer, bytes_to_read);
}

int Roboclaw::writeCommand(Command command)
{
uint8_t buffer[2];

// Just address + command ID
buffer[0] = (uint8_t)_param_rbclw_address.get();
buffer[1] = static_cast<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 -1;
return ERROR;
}

int bytes_read = read(_uart_fd, &read_buffer[total_bytes_read], bytes_to_read - total_bytes_read);

if (bytes_read <= 0) {
PX4_ERR("Read timeout %d\n", select_status);
return -1;
return ERROR;
}

total_bytes_read += bytes_read;
}

if (total_bytes_read < 2) {
PX4_ERR("Too short payload received\n");
return -1;
return ERROR;
}

// Verify checksum
// Verify response checksum
uint8_t address = static_cast<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];
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 (checksum_calc != checksum_recv) {
if (crc_calculated != crc_received) {
PX4_ERR("Checksum mismatch\n");
return -1;
return ERROR;
}

return total_bytes_read;
}

void Roboclaw::sendTransaction(e_command cmd, uint8_t *write_buffer, size_t bytes_to_write)
uint16_t Roboclaw::_calcCRC(const uint8_t *buffer, size_t bytes, uint16_t init)
{
writeCommandWithPayload(cmd, write_buffer, bytes_to_write);
int select_status = select(_uart_fd + 1, &_uart_fd_set, nullptr, nullptr, &_uart_fd_timeout);
uint16_t crc = init;

if (select_status <= 0) {
PX4_ERR("ACK timeout");
return;
}
for (size_t byte = 0; byte < bytes; byte++) {
crc = crc ^ (((uint16_t) buffer[byte]) << 8);

uint8_t acknowledgement{0};
int bytes_read = read(_uart_fd, &acknowledgement, 1);
for (uint8_t bit = 0; bit < 8; bit++) {
if (crc & 0x8000) {
crc = (crc << 1) ^ 0x1021;

if ((bytes_read != 1) || (acknowledgement != 0xFF)) {
PX4_ERR("ACK wrong");
return;
} else {
crc = crc << 1;
}
}
}

return crc;
}

Roboclaw::RoboClawError Roboclaw::writeCommand(e_command cmd)
int32_t Roboclaw::swapBytesInt32(uint8_t *buffer)
{
uint8_t buffer[2];

buffer[0] = (uint8_t)_param_rbclw_address.get();
buffer[1] = cmd;

size_t count = write(_uart_fd, buffer, 2);

if (count < 2) {
PX4_ERR("Only wrote %d out of %zu bytes", count, 2);
return RoboClawError::WriteError;
}

return RoboClawError::Success;
return (buffer[0] << 24)
| (buffer[1] << 16)
| (buffer[2] << 8)
| buffer[3];
}

Roboclaw::RoboClawError Roboclaw::writeCommandWithPayload(e_command cmd, uint8_t *wbuff, size_t bytes_to_write)
int Roboclaw::task_spawn(int argc, char *argv[])
{
size_t packet_size = 2 + bytes_to_write + 2;
uint8_t buffer[packet_size];
const char *device_name = argv[1];
const char *baud_rate_parameter_value = argv[2];

// Add address + command ID
buffer[0] = (uint8_t) _param_rbclw_address.get();
buffer[1] = cmd;
Roboclaw *instance = new Roboclaw(device_name, baud_rate_parameter_value);

// Add payload
if (bytes_to_write > 0 && wbuff) {
memcpy(&buffer[2], wbuff, bytes_to_write);
}
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
instance->ScheduleNow();
return OK;

// Add checksum
uint16_t sum = _calcCRC(buffer, packet_size - 2);
buffer[packet_size - 2] = (sum >> 8) & 0xFF;
buffer[packet_size - 1] = sum & 0xFFu;
} else {
PX4_ERR("alloc failed");
}

// Write to device
size_t count = write(_uart_fd, buffer, packet_size);
delete instance;
_object.store(nullptr);
_task_id = -1;

// Not all bytes sent
if (count < packet_size) {
PX4_ERR("Only wrote %d out of %d bytes", count, bytes_to_write);
return RoboClawError::WriteError;
}
printf("Ending task_spawn");

return RoboClawError::Success;
return ERROR;
}

int Roboclaw::custom_command(int argc, char *argv[])
@@ -562,7 +536,6 @@ The command to start this driver is: `$ roboclaw start <UART device> <baud rate>

int Roboclaw::print_status()
{
PX4_ERR("Running, Initialized: %f", (double)_initialized);
return 0;
}

189 changes: 51 additions & 138 deletions src/drivers/roboclaw/Roboclaw.hpp
Original file line number Diff line number Diff line change
@@ -32,9 +32,9 @@
****************************************************************************/

/**
* @file RoboClas.hpp
* @file Roboclaw.hpp
*
* Roboclaw Motor Driver
* Roboclaw motor control driver
*
* Product page: https://www.basicmicro.com/motor-controller
* Manual: https://downloads.basicmicro.com/docs/roboclaw_user_manual.pdf
@@ -54,173 +54,86 @@
#include <uORB/Publication.hpp>
#include <uORB/topics/wheel_encoders.h>

/**
* This is a driver for the Roboclaw motor controller
*/
class Roboclaw : public ModuleBase<Roboclaw>, public OutputModuleInterface
{
public:
/**
* constructor
* @param deviceName the name of the
* serial port e.g. "/dev/ttyS2"
* @param address the adddress of the motor
* (selectable on roboclaw)
* @param baudRateParam Name of the parameter that holds the baud rate of this serial port
*/
Roboclaw(const char *deviceName, const char *baudRateParam);

/**
* deconstructor
* @param device_name Name of the serial port e.g. "/dev/ttyS2"
* @param bad_rate_parameter Name of the parameter that holds the baud rate of this serial port
*/
Roboclaw(const char *device_name, const char *bad_rate_parameter);
virtual ~Roboclaw();

/** control channels */
enum e_channel {
CH_VOLTAGE_LEFT = 0,
CH_VOLTAGE_RIGHT
};

/** motors */
enum e_motor {
MOTOR_1 = 0,
MOTOR_2
enum class Motor {
Left = 0,
Right = 1
};

/** error handeling*/
enum class RoboClawError {
Success,
WriteError,
ReadError,
ChecksumError,
ChecksumMismatch,
UnexpectedError,
ReadTimeout
};

/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);

/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);

/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);

/** @see ModuleBase::print_status() */
int print_status() override;
static int task_spawn(int argc, char *argv[]); ///< @see ModuleBase
static int custom_command(int argc, char *argv[]); ///< @see ModuleBase
static int print_usage(const char *reason = nullptr); ///< @see ModuleBase
int print_status() override; ///< @see ModuleBase

void Run() override;

int init();

/** @see OutputModuleInterface */
bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated) override;

/**
* set the speed of a motor, rev/sec
*/
void setMotorSpeed(e_motor motor, float value);

/**
* set the duty cycle of a motor
*/
void setMotorDutyCycle(e_motor motor, float value);

/**
* Drive both motors. +1 = full forward, -1 = full backward
*/
void drive(float value);

/**
* Turn. +1 = full right, -1 = full left
*/
void turn(float value);

/**
* reset the encoders
* @return status
*/
void resetEncoders();

/**
* read data from serial
*/
void setMotorSpeed(Motor motor, float value); ///< rev/sec
void setMotorDutyCycle(Motor motor, float value);
int readEncoder();
void resetEncoders();

private:
static constexpr int MAX_ACTUATORS = 2;
enum class Command : uint8_t {
ReadStatus = 90,

DriveForwardMotor1 = 0,
DriveBackwardsMotor1 = 1,
DriveForwardMotor2 = 4,
DriveBackwardsMotor2 = 5,
DutyCycleMotor1 = 32,
DutyCycleMotor2 = 33,

ReadSpeedMotor1 = 18,
ReadSpeedMotor2 = 19,
ResetEncoders = 20,
ReadEncoderCounters = 78,
};

static constexpr int MAX_ACTUATORS = 2;
MixingOutput _mixing_output{"ROBOCLAW", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false};

char _storedDeviceName[256]; // Adjust size as necessary
char _storedBaudRateParam[256]; // Adjust size as necessary

int _timeout_counter = 0;

bool _successfully_connected{false};

// commands
// We just list the commands we want from the manual here.
enum e_command {
uORB::SubscriptionData<actuator_armed_s> _actuator_armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Publication<wheel_encoders_s> _wheel_encoders_pub{ORB_ID(wheel_encoders)};

// simple
CMD_DRIVE_FWD_1 = 0,
CMD_DRIVE_REV_1 = 1,
CMD_DRIVE_FWD_2 = 4,
CMD_DRIVE_REV_2 = 5,
char _stored_device_name[256]; // Adjust size as necessary
char _stored_baud_rate_parameter[256]; // Adjust size as necessary

CMD_DRIVE_FWD_MIX = 8,
CMD_DRIVE_REV_MIX = 9,
CMD_TURN_RIGHT = 10,
CMD_TURN_LEFT = 11,
void sendUnsigned7Bit(Command command, float data);
void sendSigned16Bit(Command command, float data);

// encoder commands
CMD_READ_ENCODER_1 = 16,
CMD_READ_ENCODER_2 = 17,
CMD_READ_SPEED_1 = 18,
CMD_READ_SPEED_2 = 19,
CMD_RESET_ENCODERS = 20,
CMD_READ_BOTH_ENCODERS = 78,
CMD_READ_BOTH_SPEEDS = 79,
// Roboclaw protocol
int sendTransaction(Command cmd, uint8_t *write_buffer, size_t bytes_to_write);
int writeCommandWithPayload(Command cmd, uint8_t *wbuff, size_t bytes_to_write);
int readAcknowledgement();

// advanced motor control
CMD_READ_SPEED_HIRES_1 = 30,
CMD_READ_SPEED_HIRES_2 = 31,
CMD_SIGNED_DUTYCYCLE_1 = 32,
CMD_SIGNED_DUTYCYCLE_2 = 33,
int receiveTransaction(Command cmd, uint8_t *read_buffer, size_t bytes_to_read);
int writeCommand(Command cmd);
int readResponse(Command command, uint8_t *read_buffer, size_t bytes_to_read);

CMD_READ_STATUS = 90
};
static uint16_t _calcCRC(const uint8_t *buf, size_t n, uint16_t init = 0);
int32_t swapBytesInt32(uint8_t *buffer);

// UART handling
int initializeUART();
bool _uart_initialized{false};
int _uart_fd{0};
fd_set _uart_fd_set;
struct timeval _uart_fd_timeout;

uORB::SubscriptionData<actuator_armed_s> _actuator_armed_sub{ORB_ID(actuator_armed)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};

matrix::Vector2f _motor_control{0.0f, 0.0f};

uORB::Publication<wheel_encoders_s> _wheel_encoders_pub {ORB_ID(wheel_encoders)};

void _parameters_update();

static uint16_t _calcCRC(const uint8_t *buf, size_t n, uint16_t init = 0);
void _sendUnsigned7Bit(e_command command, float data);
void _sendSigned16Bit(e_command command, float data);

int32_t reverseInt32(uint8_t *buffer);

bool _initialized{false};

RoboClawError writeCommand(e_command cmd);
RoboClawError writeCommandWithPayload(e_command cmd, uint8_t *wbuff, size_t bytes_to_write);

void sendTransaction(e_command cmd, uint8_t *write_buffer, size_t bytes_to_write);
int receiveTransaction(e_command cmd, uint8_t *read_buffer, size_t bytes_to_read);


DEFINE_PARAMETERS(
(ParamInt<px4::params::RBCLW_ADDRESS>) _param_rbclw_address,
(ParamInt<px4::params::RBCLW_COUNTS_REV>) _param_rbclw_counts_rev