diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 4ba2a0bb48..341d946554 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -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 * modification, are permitted provided that the following conditions @@ -41,6 +41,7 @@ * */ + #include "RoboClaw.hpp" #include #include @@ -74,321 +75,235 @@ // Number of bytes for commands 18 and 19, read speeds. #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; -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); _storedDeviceName[sizeof(_storedDeviceName) - 1] = '\0'; // Ensure null-termination strncpy(_storedBaudRateParam, baudRateParam, sizeof(_storedBaudRateParam) - 1); _storedBaudRateParam[sizeof(_storedBaudRateParam) - 1] = '\0'; // Ensure null-termination - - initialize(); } RoboClaw::~RoboClaw() { - setMotorDutyCycle(MOTOR_1, 0.0); - setMotorDutyCycle(MOTOR_2, 0.0); - close(_uart); + close(_uart_fd); } -void -RoboClaw::initialize() { +int RoboClaw::init() { + _uart_fd_timeout = { .tv_sec = 0, .tv_usec = TIMEOUT_US }; - _uart = 0; - _uart_timeout = { .tv_sec = 0, .tv_usec = TIMEOUT_US }; - _actuatorsSub = -1; - _lastEncoderCount[0] = 0; - _lastEncoderCount[1] = 0; - _encoderCounts[0] = 0; - _encoderCounts[1] = 0; - _motorSpeeds[0] = 0; - _motorSpeeds[1] = 0; + int32_t baud_rate_parameter_value{0}; + int32_t baud_rate_posix{0}; + param_get(param_find(_storedBaudRateParam), &baud_rate_parameter_value); - _param_handles.actuator_write_period_ms = param_find("RBCLW_WRITE_PER"); - _param_handles.encoder_read_period_ms = param_find("RBCLW_READ_PER"); - _param_handles.counts_per_rev = param_find("RBCLW_COUNTS_REV"); - printf("Baudrate parameter: %s\n", _storedBaudRateParam); - _param_handles.serial_baud_rate = param_find(_storedBaudRateParam); - _param_handles.address = param_find("RBCLW_ADDRESS"); + switch (baud_rate_parameter_value) { + case 0: // Auto + default: + PX4_ERR("Please configure the port's baud_rate_parameter_value"); + break; - _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 - _uart = open(_storedDeviceName, O_RDWR | O_NOCTTY); - - if (_uart < 0) { err(1, "could not open %s", _storedDeviceName); } - - // PX4_ERR("uart connection %f", (double)_uart); + _uart_fd = open(_storedDeviceName, O_RDWR | O_NOCTTY); + if (_uart_fd < 0) { err(1, "could not open %s", _storedDeviceName); } int ret = 0; struct termios uart_config {}; - ret = tcgetattr(_uart, &uart_config); + ret = tcgetattr(_uart_fd, &uart_config); if (ret < 0) { err(1, "failed to get attr"); } uart_config.c_oflag &= ~ONLCR; // no CR for every LF uart_config.c_cflag &= ~CRTSCTS; - ret = cfsetispeed(&uart_config, _parameters.serial_baud_rate); + // Set baud rate + ret = cfsetispeed(&uart_config, baud_rate_posix); 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"); } - ret = tcsetattr(_uart, TCSANOW, &uart_config); - + ret = tcsetattr(_uart_fd, TCSANOW, &uart_config); if (ret < 0) { err(1, "failed to set attr"); } - FD_ZERO(&_uart_set); - FD_SET(_uart, &_uart_set); + FD_ZERO(&_uart_fd_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. - uint8_t rbuff[CMD_READ_STATUS_MESSAGE_SIZE]; - int err_code = _transaction(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true); + 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)); - if (err_code < 0) { + if (err_code <= 0) { PX4_ERR("Shutting down Roboclaw driver."); - return; + return PX4_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"); } - // This main loop performs two different tasks, asynchronously: - // - Send actuator_controls_0 to the Roboclaw as soon as they are available - // - Read the encoder values at a constant rate - // To do this, the timeout on the poll() function is used. - // waitTime is the amount of time left (int microseconds) until the next time I should read from the encoders. - // It is updated at the end of every loop. Sometimes, if the actuator_controls_0 message came in right before - // I should have read the encoders, waitTime will be 0. This is fine. When waitTime is 0, poll() will return - // immediately with a timeout. (Or possibly with a message, if one happened to be available at that exact moment) + delete instance; + _object.store(nullptr); + _task_id = -1; - // 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)); - orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms); - - _armedSub = orb_subscribe(ORB_ID(actuator_armed)); - _paramSub = orb_subscribe(ORB_ID(parameter_update)); - - pollfd fds[3]; - fds[0].fd = _paramSub; - fds[0].events = POLLIN; - fds[1].fd = _actuatorsSub; - fds[1].events = POLLIN; - fds[2].fd = _armedSub; - fds[2].events = POLLIN; - - memset((void *) &_wheelEncoderMsg[0], 0, sizeof(_wheelEncoderMsg)); - _wheelEncoderMsg[0].pulses_per_rev = _parameters.counts_per_rev; - _wheelEncoderMsg[1].pulses_per_rev = _parameters.counts_per_rev; - - while (!taskShouldExit) { - - // 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"); - } - - } +void RoboClaw::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + _mixing_output.unregister(); + return; } + _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() { + 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]; - // I am saving space by overlapping the two separate motor speeds, so that the final buffer will look like: - // [ ] - // And I just ignore all of the statuses and checksums. (The _transaction() function internally handles the - // checksum) - uint8_t rbuff_speed[ENCODER_SPEED_MESSAGE_SIZE + 4]; - - bool success = false; - - for (int retry = 0; retry < TIMEOUT_RETRIES && !success; retry++) { - success = _transaction(CMD_READ_BOTH_ENCODERS, nullptr, 0, &rbuff_pos[0], ENCODER_MESSAGE_SIZE, false, - true) > 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) { + if (receiveTransaction(CMD_READ_BOTH_ENCODERS, buffer_positon, ENCODER_MESSAGE_SIZE) < ENCODER_MESSAGE_SIZE) { return -1; } - uint32_t count; - uint32_t speed; - uint8_t *count_bytes; - uint8_t *speed_bytes; - - for (int motor = 0; motor <= 1; motor++) { - count = 0; - speed = 0; - count_bytes = &rbuff_pos[motor * 4]; - speed_bytes = &rbuff_speed[motor * 4]; - - // Data from the roboclaw is big-endian. This converts the bytes to an integer, regardless of the - // endianness of the system this code is running on. - for (int byte = 0; byte < 4; byte++) { - count = (count << 8) + count_bytes[byte]; - speed = (speed << 8) + speed_bytes[byte]; - } - - // The Roboclaw stores encoder counts as unsigned 32-bit ints. This can overflow, especially when starting - // at 0 and moving backward. The Roboclaw has overflow flags for this, but in my testing, they don't seem - // to work. This code detects overflow manually. - // These diffs are the difference between the count I just read from the Roboclaw and the last - // count that was read from the roboclaw for this motor. fwd_diff assumes that the wheel moved forward, - // and rev_diff assumes it moved backward. If the motor actually moved forward, then rev_diff will be close - // to 2^32 (UINT32_MAX). If the motor actually moved backward, then fwd_diff will be close to 2^32. - // To detect and account for overflow, I just assume that the smaller diff is correct. - // Strictly speaking, if the wheel rotated more than 2^31 encoder counts since the last time I checked, this - // will be wrong. But that's 1.7 million revolutions, so it probably won't come up. - uint32_t fwd_diff = count - _lastEncoderCount[motor]; - uint32_t rev_diff = _lastEncoderCount[motor] - count; - // At this point, abs(diff) is always <= 2^31, so this cast from unsigned to signed is safe. - int32_t diff = fwd_diff <= rev_diff ? fwd_diff : -int32_t(rev_diff); - _encoderCounts[motor] += diff; - _lastEncoderCount[motor] = count; - - _motorSpeeds[motor] = speed; + if (receiveTransaction(CMD_READ_SPEED_1, buffer_speed_right, ENCODER_SPEED_MESSAGE_SIZE) < ENCODER_SPEED_MESSAGE_SIZE) { + return -1; } + 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(position_right) / _param_rbclw_counts_rev.get() * M_TWOPI_F; + wheel_encoders.wheel_angle[1] = static_cast(position_left) / _param_rbclw_counts_rev.get() * M_TWOPI_F; + wheel_encoders.wheel_speed[0] = static_cast(speed_right) / _param_rbclw_counts_rev.get() * M_TWOPI_F; + wheel_encoders.wheel_speed[1] = static_cast(speed_left) / _param_rbclw_counts_rev.get() * M_TWOPI_F; + wheel_encoders.timestamp = hrt_absolute_time(); + _wheel_encoders_pub.publish(wheel_encoders); + 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", - double(getMotorPosition(MOTOR_1)), - double(getMotorSpeed(MOTOR_1)), - double(getMotorPosition(MOTOR_2)), - double(getMotorSpeed(MOTOR_2))); + return (buffer[0] << 24) + | (buffer[1] << 16) + | (buffer[2] << 8) + | buffer[3]; } -float RoboClaw::getMotorPosition(e_motor motor) -{ - if (motor == MOTOR_1) { - return _encoderCounts[0]; - - } else if (motor == MOTOR_2) { - return _encoderCounts[1]; - - } else { - warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); - return NAN; - } -} - -float RoboClaw::getMotorSpeed(e_motor motor) -{ - if (motor == MOTOR_1) { - return _motorSpeeds[0]; - - } else if (motor == MOTOR_2) { - return _motorSpeeds[1]; - - } else { - warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); - return NAN; - } -} - -int RoboClaw::setMotorSpeed(e_motor motor, float value) +void RoboClaw::setMotorSpeed(e_motor motor, float value) { e_command command; @@ -410,15 +325,14 @@ int RoboClaw::setMotorSpeed(e_motor motor, float value) } } 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; // send command @@ -429,56 +343,48 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value) command = CMD_SIGNED_DUTYCYCLE_2; } else { - return -1; + return; } 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; - 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; - 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); - if (data > 1.0f) { - data = 1.0f; + if (data >= 1.0f) { + data = 0.99f; } auto byte = (uint8_t)(data * INT8_MAX); - uint8_t recv_byte; - return _transaction(command, &byte, 1, &recv_byte, 1); + sendTransaction(command, &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; uint8_t buff[2]; buff[0] = (duty >> 8) & 0xFF; // High byte buff[1] = duty & 0xFF; // Low byte - uint8_t recv_buff; - return _transaction(command, (uint8_t *) &buff, 2, &recv_buff, 2); -} - -int RoboClaw::_sendNothing(e_command command) -{ - uint8_t recv_buff; - return _transaction(command, nullptr, 0, &recv_buff, 1); + sendTransaction(command, (uint8_t *) &buff, 2); } 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; } -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) { - - // 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); + if (writeCommand(cmd) != RoboClawError::Success) { return -1; } - err_code = readFromDevice(rbuff, rbytes, recv_checksum, buf); + size_t total_bytes_read = 0; - if (err_code != RoboClawError::Success) { - printError(err_code); + 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; } - return 1; + // Verify checksum + uint8_t address = static_cast(_param_rbclw_address.get()); + uint8_t command = static_cast(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 (wbuff) { - memcpy(&buf[2], wbuff, wbytes); + if (select_status <= 0) { + PX4_ERR("ACK timeout"); + return; } - // Compute and append checksum if necessary. - wbytes = wbytes + (send_checksum ? 4 : 2); + uint8_t acknowledgement{0}; + int bytes_read = read(_uart_fd, &acknowledgement, 1); - if (send_checksum) { - uint16_t sum = _calcCRC(buf, wbytes - 2); - buf[wbytes - 2] = (sum >> 8) & 0xFF; - buf[wbytes - 1] = sum & 0xFFu; + if ((bytes_read != 1) || (acknowledgement != 0xFF)) { + PX4_ERR("ACK wrong"); + return; } +} - // 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. - if (count < (int) wbytes) { // Did not successfully send all bytes. - PX4_ERR("Only wrote %d out of %zu bytes", count, wbytes); + 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 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; - size_t bytes_read = 0; + // Add address + command ID + buffer[0] = (uint8_t) _param_rbclw_address.get(); + buffer[1] = cmd; - while (bytes_read < rbytes) { - - _uart_timeout.tv_sec = 0; - _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; + // Add payload + if (bytes_to_write > 0 && wbuff) { + memcpy(&buffer[2], wbuff, bytes_to_write); } - 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) { - return RoboClawError::ChecksumError; // or an equivalent error code of your choosing - } + // Write to device + size_t count = write(_uart_fd, buffer, packet_size); - uint16_t checksum_calc = _calcCRC(buf, 2); // assuming buf contains address and command - checksum_calc = _calcCRC(rbuff, bytes_read - 2, checksum_calc); - uint16_t checksum_recv = (rbuff[bytes_read - 2] << 8) + rbuff[bytes_read - 1]; - - if (checksum_calc != checksum_recv) { - return RoboClawError::ChecksumError; - } + // 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; // Return success if everything went well + return RoboClawError::Success; } -void RoboClaw::printError(RoboClawError error) +int RoboClaw::custom_command(int argc, char *argv[]) { - - 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"); - } + return 0; } - -void RoboClaw::_parameters_update() +int RoboClaw::print_usage(const char *reason) { - param_get(_param_handles.counts_per_rev, &_parameters.counts_per_rev); - param_get(_param_handles.encoder_read_period_ms, &_parameters.encoder_read_period_ms); - param_get(_param_handles.actuator_write_period_ms, &_parameters.actuator_write_period_ms); - param_get(_param_handles.address, &_parameters.address); - - if (_actuatorsSub > 0) { - orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms); - } - - int32_t baudRate; - param_get(_param_handles.serial_baud_rate, &baudRate); - - switch (baudRate) { - case 2400: - _parameters.serial_baud_rate = B2400; - break; - - case 9600: - _parameters.serial_baud_rate = B9600; - break; - - case 19200: - _parameters.serial_baud_rate = B19200; - break; - - case 38400: - _parameters.serial_baud_rate = B38400; - break; - - case 57600: - _parameters.serial_baud_rate = B57600; - break; - - case 115200: - _parameters.serial_baud_rate = B115200; - break; - - case 230400: - _parameters.serial_baud_rate = B230400; - break; - - case 460800: - _parameters.serial_baud_rate = B460800; - break; - - default: - _parameters.serial_baud_rate = B2400; - } + return 0; +} + +int RoboClaw::print_status() +{ + PX4_ERR("Running, Initialized: %f", (double)_initialized); + return 0; +} + +extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]) +{ + return RoboClaw::main(argc, argv); } diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index abbb339857..681594eb24 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -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 * modification, are permitted provided that the following conditions @@ -53,25 +53,45 @@ #include #include +#include + #include -#include #include #include #include -#include -#include -#include +#include + +#include + +#include +#include +#include +#include +#include + +#include /** * This is a driver for the RoboClaw motor controller */ -class RoboClaw +class RoboClaw : public ModuleBase, 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); - void taskMain(); - static bool taskShouldExit; + /** + * deconstructor + */ + virtual ~RoboClaw(); /** control channels */ enum e_channel { @@ -96,77 +116,68 @@ public: ReadTimeout }; - /** - * 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); + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); - /** - * deconstructor - */ - virtual ~RoboClaw(); + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); - void initialize(); + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); - /** - * @return position of a motor, rev - */ - float getMotorPosition(e_motor motor); + /** @see ModuleBase::print_status() */ + int print_status() override; - /** - * @return speed of a motor, rev/sec - */ - float getMotorSpeed(e_motor motor); + void Run() override; + + int init(); + + 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 */ - int setMotorSpeed(e_motor motor, float value); + void setMotorSpeed(e_motor motor, float value); /** * 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 */ - int drive(float value); + void drive(float value); /** * Turn. +1 = full right, -1 = full left */ - int turn(float value); + void turn(float value); /** * reset the encoders * @return status */ - int resetEncoders(); + void resetEncoders(); /** * read data from serial */ int readEncoder(); - /** - * print status - */ - void printStatus(); - - 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 _storedBaudRateParam[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 { @@ -200,99 +211,39 @@ private: CMD_READ_STATUS = 90 }; + int _uart_fd{0}; + fd_set _uart_fd_set; + struct timeval _uart_fd_timeout; - struct { - speed_t serial_baud_rate; - int32_t counts_per_rev; - int32_t encoder_read_period_ms; - int32_t actuator_write_period_ms; - int32_t address; - } _parameters{}; + uORB::SubscriptionData _actuator_armed_sub{ORB_ID(actuator_armed)}; + uORB::Subscription _differential_drive_control_sub{ORB_ID(differential_drive_control)}; + uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; - struct { - param_t serial_baud_rate; - param_t counts_per_rev; - param_t encoder_read_period_ms; - param_t actuator_write_period_ms; - param_t address; - } _param_handles{}; + differential_drive_control_s _diff_drive_control{}; - int _uart; - fd_set _uart_set; - struct timeval _uart_timeout; + matrix::Vector2f _motor_control{0.0f, 0.0f}; - /** actuator controls subscription */ - int _actuatorsSub{-1}; - actuator_controls_s _actuatorControls; - - int _armedSub{-1}; - actuator_armed_s _actuatorArmed; - - int _paramSub{-1}; - parameter_update_s _paramUpdate; - - uORB::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 _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}; + uORB::Publication _wheel_encoders_pub {ORB_ID(wheel_encoders)}; void _parameters_update(); - void vehicle_control_poll(); static uint16_t _calcCRC(const uint8_t *buf, size_t n, uint16_t init = 0); - int _sendUnsigned7Bit(e_command command, float data); - int _sendSigned16Bit(e_command command, float data); - int _sendNothing(e_command); + 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); - /** - * print status - */ - 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); - + DEFINE_PARAMETERS( + (ParamInt) _param_rbclw_address, + (ParamInt) _param_rbclw_counts_rev + ) };