forked from Archive/PX4-Autopilot
Driving possible
This commit is contained in:
Matthias Grob
parent
ab486de430
commit
a857df88e4
|
|
@ -55,6 +55,7 @@
|
|||
#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
|
||||
|
|
@ -75,6 +76,8 @@
|
|||
|
||||
bool RoboClaw::taskShouldExit = false;
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam):
|
||||
_uart(0),
|
||||
_uart_set(),
|
||||
|
|
@ -88,6 +91,7 @@ RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam):
|
|||
_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", baudRateParam);
|
||||
_param_handles.serial_baud_rate = param_find(baudRateParam);
|
||||
_param_handles.address = param_find("RBCLW_ADDRESS");
|
||||
|
||||
|
|
@ -105,6 +109,7 @@ RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam):
|
|||
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);
|
||||
|
||||
if (ret < 0) { err(1, "failed to set input speed"); }
|
||||
|
|
@ -118,15 +123,16 @@ RoboClaw::RoboClaw(const char *deviceName, const char *baudRateParam):
|
|||
if (ret < 0) { err(1, "failed to set attr"); }
|
||||
|
||||
FD_ZERO(&_uart_set);
|
||||
FD_SET(_uart, &_uart_set);
|
||||
|
||||
// setup default settings, reset encoders
|
||||
resetEncoders();
|
||||
//resetEncoders();
|
||||
}
|
||||
|
||||
RoboClaw::~RoboClaw()
|
||||
{
|
||||
setMotorDutyCycle(MOTOR_1, 0.0);
|
||||
setMotorDutyCycle(MOTOR_2, 0.0);
|
||||
// setMotorDutyCycle(MOTOR_1, 0.0);
|
||||
// setMotorDutyCycle(MOTOR_2, 0.0);
|
||||
close(_uart);
|
||||
}
|
||||
|
||||
|
|
@ -149,10 +155,7 @@ void RoboClaw::taskMain()
|
|||
// It is updated at the end of every loop. Sometimes, if the actuator_controls_0 message came in right before
|
||||
// I should have read the encoders, waitTime will be 0. This is fine. When waitTime is 0, poll() will return
|
||||
// immediately with a timeout. (Or possibly with a message, if one happened to be available at that exact moment)
|
||||
uint64_t encoderTaskLastRun = 0;
|
||||
int waitTime = 0;
|
||||
|
||||
uint64_t actuatorsLastWritten = 0;
|
||||
int waitTime = 100_ms;
|
||||
|
||||
_actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0));
|
||||
orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms);
|
||||
|
|
@ -168,74 +171,35 @@ void RoboClaw::taskMain()
|
|||
fds[2].fd = _armedSub;
|
||||
fds[2].events = POLLIN;
|
||||
|
||||
memset((void *) &_wheelEncoderMsg[0], 0, sizeof(_wheelEncoderMsg));
|
||||
_wheelEncoderMsg[0].pulses_per_rev = _parameters.counts_per_rev;
|
||||
_wheelEncoderMsg[1].pulses_per_rev = _parameters.counts_per_rev;
|
||||
|
||||
while (!taskShouldExit) {
|
||||
|
||||
int pret = poll(fds, sizeof(fds) / sizeof(pollfd), waitTime / 1000);
|
||||
|
||||
bool actuators_timeout = int(hrt_absolute_time() - actuatorsLastWritten) > 2000 * _parameters.actuator_write_period_ms;
|
||||
|
||||
if (fds[0].revents & POLLIN) {
|
||||
orb_copy(ORB_ID(parameter_update), _paramSub, &_paramUpdate);
|
||||
_parameters_update();
|
||||
}
|
||||
|
||||
// No timeout, update on either the actuator controls or the armed state
|
||||
if (pret > 0 && (fds[1].revents & POLLIN || fds[2].revents & POLLIN || actuators_timeout)) {
|
||||
if (pret > 0 && (fds[1].revents & POLLIN || fds[2].revents & POLLIN)) {
|
||||
orb_copy(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuatorControls);
|
||||
orb_copy(ORB_ID(actuator_armed), _armedSub, &_actuatorArmed);
|
||||
|
||||
int drive_ret = 0, turn_ret = 0;
|
||||
|
||||
const bool disarmed = !_actuatorArmed.armed || _actuatorArmed.lockdown || _actuatorArmed.manual_lockdown
|
||||
|| _actuatorArmed.force_failsafe || actuators_timeout;
|
||||
|| _actuatorArmed.force_failsafe;
|
||||
|
||||
if (disarmed) {
|
||||
// If disarmed, I want to be certain that the stop command gets through.
|
||||
int tries = 0;
|
||||
|
||||
while (tries < STOP_RETRIES && ((drive_ret = drive(0.0)) <= 0 || (turn_ret = turn(0.0)) <= 0)) {
|
||||
PX4_ERR("Error trying to stop: Drive: %d, Turn: %d", drive_ret, turn_ret);
|
||||
tries++;
|
||||
px4_usleep(TIMEOUT_US);
|
||||
}
|
||||
setMotorDutyCycle(MOTOR_1, 0.f);
|
||||
setMotorDutyCycle(MOTOR_2, 0.f);
|
||||
|
||||
} else {
|
||||
drive_ret = drive(_actuatorControls.control[actuator_controls_s::INDEX_THROTTLE]);
|
||||
turn_ret = turn(_actuatorControls.control[actuator_controls_s::INDEX_YAW]);
|
||||
|
||||
if (drive_ret <= 0 || turn_ret <= 0) {
|
||||
PX4_ERR("Error controlling RoboClaw. Drive err: %d. Turn err: %d", drive_ret, turn_ret);
|
||||
}
|
||||
}
|
||||
|
||||
actuatorsLastWritten = hrt_absolute_time();
|
||||
|
||||
} else {
|
||||
// A timeout occurred, which means that it's time to update the encoders
|
||||
encoderTaskLastRun = hrt_absolute_time();
|
||||
|
||||
if (readEncoder() > 0) {
|
||||
|
||||
for (int i = 0; i < 2; i++) {
|
||||
_wheelEncoderMsg[i].timestamp = encoderTaskLastRun;
|
||||
|
||||
_wheelEncoderMsg[i].encoder_position = _encoderCounts[i];
|
||||
_wheelEncoderMsg[i].speed = _motorSpeeds[i];
|
||||
|
||||
_wheelEncodersAdv[i].publish(_wheelEncoderMsg[i]);
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("Error reading encoders");
|
||||
const float throttle = _actuatorControls.control[actuator_controls_s::INDEX_THROTTLE];
|
||||
const float yaw = _actuatorControls.control[actuator_controls_s::INDEX_YAW];
|
||||
const float scale = 0.3f;
|
||||
setMotorDutyCycle(MOTOR_1, (throttle - yaw) * scale);
|
||||
setMotorDutyCycle(MOTOR_2, (throttle + yaw) * scale);
|
||||
}
|
||||
}
|
||||
|
||||
waitTime = _parameters.encoder_read_period_ms * 1000 - (hrt_absolute_time() - encoderTaskLastRun);
|
||||
waitTime = waitTime < 0 ? 0 : waitTime;
|
||||
}
|
||||
|
||||
orb_unsubscribe(_actuatorsSub);
|
||||
|
|
@ -426,16 +390,12 @@ int RoboClaw::_sendUnsigned7Bit(e_command command, float data)
|
|||
|
||||
int RoboClaw::_sendSigned16Bit(e_command command, float data)
|
||||
{
|
||||
if (data > 1.0f) {
|
||||
data = 1.0f;
|
||||
|
||||
} else if (data < -1.0f) {
|
||||
data = -1.0f;
|
||||
}
|
||||
|
||||
auto buff = (uint16_t)(data * INT16_MAX);
|
||||
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, 1);
|
||||
return _transaction(command, (uint8_t *) &buff, 2, &recv_buff, 2);
|
||||
}
|
||||
|
||||
int RoboClaw::_sendNothing(e_command command)
|
||||
|
|
@ -488,6 +448,14 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
|
|||
buf[wbytes - 1] = sum & 0xFFu;
|
||||
}
|
||||
|
||||
// printf("Write: ");
|
||||
|
||||
// for (size_t i = 0; i < wbytes; i++) {
|
||||
// printf("%X ", buf[i]);
|
||||
// }
|
||||
|
||||
// printf("\n");
|
||||
|
||||
int count = write(_uart, buf, wbytes);
|
||||
|
||||
if (count < (int) wbytes) { // Did not successfully send all bytes.
|
||||
|
|
@ -495,30 +463,37 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
|
|||
return -1;
|
||||
}
|
||||
|
||||
// else {
|
||||
// printf("Successfully wrote %d of %zu bytes\n", count, wbytes);
|
||||
// }
|
||||
|
||||
// READ
|
||||
|
||||
FD_ZERO(&_uart_set);
|
||||
FD_SET(_uart, &_uart_set);
|
||||
|
||||
uint8_t *rbuff_curr = rbuff;
|
||||
size_t bytes_read = 0;
|
||||
|
||||
// select(...) returns as soon as even 1 byte is available. read(...) returns immediately, no matter how many
|
||||
// bytes are available. I need to keep reading until I get the number of bytes I expect.
|
||||
//printf("Read: \n");
|
||||
while (bytes_read < rbytes) {
|
||||
// select(...) may change this timeout struct (because it is not const). So I reset it every time.
|
||||
_uart_timeout.tv_sec = 0;
|
||||
_uart_timeout.tv_usec = TIMEOUT_US;
|
||||
err_code = select(_uart + 1, &_uart_set, nullptr, nullptr, &_uart_timeout);
|
||||
//printf("Select: %d\n", err_code);
|
||||
|
||||
// An error code of 0 means that select timed out, which is how the Roboclaw indicates an error.
|
||||
if (err_code <= 0) {
|
||||
printf("select error: %d\n", err_code);
|
||||
return err_code;
|
||||
}
|
||||
|
||||
err_code = read(_uart, rbuff_curr, rbytes - bytes_read);
|
||||
//printf("Read: %d\n", err_code);
|
||||
|
||||
if (err_code <= 0) {
|
||||
printf("read error: %d\n", err_code);
|
||||
return err_code;
|
||||
|
||||
} else {
|
||||
|
|
@ -527,6 +502,14 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
|
|||
}
|
||||
}
|
||||
|
||||
// printf("Read: ");
|
||||
|
||||
// for (size_t i = 0; i < bytes_read; i++) {
|
||||
// printf("%X ", rbuff[i]);
|
||||
// }
|
||||
|
||||
// printf("\n");
|
||||
|
||||
//TODO: Clean up this mess of IFs and returns
|
||||
|
||||
if (recv_checksum) {
|
||||
|
|
@ -540,6 +523,7 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
|
|||
checksum_calc = _calcCRC(rbuff, bytes_read - 2, checksum_calc);
|
||||
uint16_t checksum_recv = (rbuff[bytes_read - 2] << 8) + rbuff[bytes_read - 1];
|
||||
|
||||
//printf("crc calc: %X crc rec: %X\n", checksum_calc, checksum_recv);
|
||||
if (checksum_calc == checksum_recv) {
|
||||
return bytes_read;
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,66 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file RoboclawSerialDevice.hpp
|
||||
* @brief
|
||||
* @author Matthias Grob <maetugr@gmail.com>
|
||||
*/
|
||||
|
||||
#include "RoboclawDriver/RoboclawDriver.hpp"
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
class RoboclawDevice : public RoboclawWritableInterface
|
||||
{
|
||||
public:
|
||||
RoboclawDevice(const char *port);
|
||||
~RoboclawDevice();
|
||||
int init();
|
||||
void printStatus();
|
||||
|
||||
private:
|
||||
void Run();
|
||||
size_t writeCallback(const uint8_t *buffer, const uint16_t length) override;
|
||||
int setBaudrate(const unsigned baudrate);
|
||||
|
||||
static constexpr size_t READ_BUFFER_SIZE{256};
|
||||
static constexpr int DISARM_VALUE = 0;
|
||||
static constexpr int MIN_THROTTLE = 100;
|
||||
static constexpr int MAX_THROTTLE = 1000;
|
||||
|
||||
char _port[20] {};
|
||||
int _serial_fd{-1};
|
||||
VescDriver _vesc_driver;
|
||||
MixingOutput _mixing_output{4, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
|
||||
perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
|
||||
};
|
||||
|
|
@ -3,4 +3,4 @@ serial_config:
|
|||
- command: roboclaw start ${SERIAL_DEV} ${BAUD_PARAM}
|
||||
port_config_param:
|
||||
name: RBCLW_SER_CFG
|
||||
group: Roboclaw
|
||||
group: Roboclaw
|
||||
|
|
|
|||
|
|
@ -82,11 +82,11 @@ static void usage()
|
|||
PRINT_MODULE_DESCRIPTION(R"DESCR_STR(
|
||||
### Description
|
||||
|
||||
This driver communicates over UART with the [Roboclaw motor driver](http://downloads.basicmicro.com/docs/roboclaw_user_manual.pdf).
|
||||
This driver communicates over UART with the [Roboclaw motor driver](https://www.basicmicro.com/motor-controller).
|
||||
It performs two tasks:
|
||||
|
||||
- Control the motors based on the `actuator_controls_0` UOrb topic.
|
||||
- Read the wheel encoders and publish the raw data in the `wheel_encoders` UOrb topic
|
||||
- Control the motors based on the `actuator_controls_0` uORB topic.
|
||||
- Read the wheel encoders and publish the raw data in the `wheel_encoders` uORB topic
|
||||
|
||||
In order to use this driver, the Roboclaw should be put into Packet Serial mode (see the linked documentation), and
|
||||
your flight controller's UART port should be connected to the Roboclaw as shown in the documentation. For Pixhawk 4,
|
||||
|
|
@ -132,12 +132,7 @@ All available commands are:
|
|||
*/
|
||||
int roboclaw_main(int argc, char *argv[])
|
||||
{
|
||||
|
||||
if (argc < 4) {
|
||||
usage();
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
if (!strcmp(argv[1], "start") && (argc >= 4)) {
|
||||
|
||||
if (thread_running) {
|
||||
printf("roboclaw already running\n");
|
||||
|
|
|
|||
Loading…
Reference in New Issue