Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

Added parameters

Browse Source
This commit is contained in:
Timothy Scott
2019-07-05 14:01:56 +02:00
committed by Julian Oes
parent ffe505b76b
commit 2b79684f29
4 changed files with 227 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/roboclaw/RoboClaw.cpp
+69 -34
View File
@@ -62,38 +62,12 @@
// Add a little extra to account for timing inaccuracy
#define TIMEOUT_US 10500
// TODO: Make these all parameters
#define FAILED_TRANSACTION_RETRIES 1
#define ENCODER_READ_PERIOD_MS 10
#define ACTUATOR_WRITE_PERIOD_MS 10
// Number of bytes returned by the Roboclaw when sending command 78, read both encoders
#define ENCODER_MESSAGE_SIZE 10
// TODO: Delete this
//void printbytes(const char *msg, uint8_t *bytes, int numbytes)
//{
// if (numbytes < 0) {
// numbytes = 0;
// }
//
// size_t msglen = strlen(msg);
// char buff[msglen + (4 * numbytes) + 1];
// char *cur = &buff[0];
// cur += sprintf(cur, "%s ", msg);
//
// for (int i = 0; i < numbytes; i++) {
// cur += sprintf(cur, "0x%02X ", bytes[i]);
// }
//
// PX4_INFO("%s", buff);
//}
bool RoboClaw::taskShouldExit = false;
RoboClaw::RoboClaw(const char *deviceName, uint16_t address, uint16_t pulsesPerRev):
_address(address),
_pulsesPerRev(pulsesPerRev),
RoboClaw::RoboClaw(const char *deviceName):
_uart(0),
_uart_set(),
_uart_timeout{.tv_sec = 0, .tv_usec = TIMEOUT_US},
@@ -102,6 +76,17 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address, uint16_t pulsesPerR
_motorSpeeds{0, 0}
{
_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");
_param_handles.serial_baud_rate = param_find("RMCLW_BAUD");
_param_handles.serial_timeout_retries = param_find("RBCLW_RETRIES");
_param_handles.stop_retries = param_find("RBCLW_STOP_RETRY");
_param_handles.address = param_find("RBCLW_ADDRESS");
_parameters_update();
// start serial port
_uart = open(deviceName, O_RDWR | O_NOCTTY);
@@ -114,11 +99,11 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address, uint16_t pulsesPerR
if (ret < 0) { err(1, "failed to get attr"); }
uart_config.c_oflag &= ~ONLCR; // no CR for every LF
ret = cfsetispeed(&uart_config, B38400);
ret = cfsetispeed(&uart_config, _parameters.serial_baud_rate);
if (ret < 0) { err(1, "failed to set input speed"); }
ret = cfsetospeed(&uart_config, B38400);
ret = cfsetospeed(&uart_config, _parameters.serial_baud_rate);
if (ret < 0) { err(1, "failed to set output speed"); }
@@ -154,7 +139,7 @@ void RoboClaw::taskMain()
int waitTime = 0;
_actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0));
orb_set_interval(_actuatorsSub, ACTUATOR_WRITE_PERIOD_MS);
orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms);
_armedSub = orb_subscribe(ORB_ID(actuator_armed));
@@ -186,7 +171,7 @@ void RoboClaw::taskMain()
if (disarmed) {
// If disarmed, I want to be certain that the stop command gets through.
while ((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);
//PX4_ERR("Error trying to stop: Drive: %d, Turn: %d", drive_ret, turn_ret);
px4_usleep(TIMEOUT_US);
}
@@ -218,7 +203,7 @@ void RoboClaw::taskMain()
}
}
waitTime = ENCODER_READ_PERIOD_MS * 1000 - (hrt_absolute_time() - encoderTaskLastRun);
waitTime = _parameters.encoder_read_period_ms * 1000 - (hrt_absolute_time() - encoderTaskLastRun);
waitTime = waitTime < 0 ? 0 : waitTime;
}
@@ -233,7 +218,7 @@ int RoboClaw::readEncoder()
uint8_t rbuff[ENCODER_MESSAGE_SIZE];
int nread = 0;
for (int retry = 0; retry < FAILED_TRANSACTION_RETRIES && nread == 0; retry++) {
for (int retry = 0; retry < _parameters.serial_timeout_retries && nread == 0; retry++) {
nread = _transaction(CMD_READ_BOTH_ENCODERS, nullptr, 0, &rbuff[0], ENCODER_MESSAGE_SIZE, false, true);
}
@@ -441,7 +426,7 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
tcflush(_uart, TCIOFLUSH); // flush buffers
uint8_t buf[wbytes + 4];
buf[0] = _address;
buf[0] = (uint8_t) _parameters.address;
buf[1] = cmd;
if (wbuff) {
@@ -525,3 +510,53 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
}
}
}
void RoboClaw::_parameters_update()
{
param_get(_param_handles.serial_timeout_retries, &_parameters.serial_timeout_retries);
param_get(_param_handles.stop_retries, &_parameters.stop_retries);
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);
int baudRate;
param_get(_param_handles.serial_baud_rate, &baudRate);
switch (baudRate) {
case 1:
_parameters.serial_baud_rate = B2400;
break;
case 2:
_parameters.serial_baud_rate = B9600;
break;
case 3:
_parameters.serial_baud_rate = B19200;
break;
case 4:
_parameters.serial_baud_rate = B38400;
break;
case 5:
_parameters.serial_baud_rate = B57600;
break;
case 6:
_parameters.serial_baud_rate = B115200;
break;
case 7:
_parameters.serial_baud_rate = B230400;
break;
case 8:
_parameters.serial_baud_rate = B460800;
break;
default:
_parameters.serial_baud_rate = B9600;
}
}
src/drivers/roboclaw/RoboClaw.hpp
+23 -5
View File
@@ -45,6 +45,7 @@
#include <poll.h>
#include <stdio.h>
#include <termios.h>
#include <uORB/SubscriptionPollable.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/wheel_encoders.h>
@@ -61,7 +62,6 @@ class RoboClaw
{
public:
static int roboclawTest(int argc, char *argv[]);
void taskMain();
static bool taskShouldExit;
@@ -86,8 +86,7 @@ public:
* @param pulsesPerRev # of encoder
* pulses per revolution of wheel
*/
RoboClaw(const char *deviceName, uint16_t address,
uint16_t pulsesPerRev);
RoboClaw(const char *deviceName);
/**
* deconstructor
@@ -185,8 +184,25 @@ private:
CMD_SIGNED_DUTYCYCLE_2 = 33,
};
uint8_t _address;
uint16_t _pulsesPerRev;
struct {
speed_t serial_baud_rate;
int32_t counts_per_rev;
int32_t serial_timeout_retries;
int32_t stop_retries;
int32_t encoder_read_period_ms;
int32_t actuator_write_period_ms;
int32_t address;
} _parameters{};
struct {
param_t serial_baud_rate;
param_t counts_per_rev;
param_t serial_timeout_retries;
param_t stop_retries;
param_t encoder_read_period_ms;
param_t actuator_write_period_ms;
param_t address;
} _param_handles{};
int _uart;
fd_set _uart_set;
@@ -206,6 +222,8 @@ private:
int64_t _encoderCounts[2];
int32_t _motorSpeeds[2];
void _parameters_update();
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);
src/drivers/roboclaw/roboclaw_main.cpp
+1 -1
View File
@@ -153,7 +153,7 @@ int roboclaw_thread_main(int argc, char *argv[])
deviceName, address, pulsesPerRev);
// start
RoboClaw roboclaw(deviceName, address, pulsesPerRev);
RoboClaw roboclaw(deviceName);
thread_running = true;
src/drivers/roboclaw/roboclaw_params.c
+134
View File
@@ -0,0 +1,134 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 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 roboclaw_params.c
*
* Parameters defined by the Roboclaw driver.
*
* The Roboclaw will need to be configured to match these parameters. For information about configuring the
* Roboclaw, see http://downloads.ionmc.com/docs/roboclaw_user_manual.pdf
*
* @author Timothy Scott <timothy@auterion.com>
*/
/**
* Uart write period
*
* How long to wait, in Milliseconds, between writing actuator controls over Uart to the Roboclaw
* @unit ms
* @min 1
* @max 1000
* @group Roboclaw driver
*/
PARAM_DEFINE_INT32(RBCLW_WRITE_PER, 10);
/**
* Encoder read period
*
* How long to wait, in Milliseconds, between reading wheel encoder values over Uart from the Roboclaw
* @unit ms
* @min 1
* @max 1000
* @group Roboclaw driver
*/
PARAM_DEFINE_INT32(RBCLW_READ_PER, 10);
/**
* Encoder counts per revolution
*
* Number of encoder counts for one revolution. The roboclaw treats analog encoders (potentiometers) as having 2047
* counts per rev. The default value of 1200 corresponds to the default configuration of the Aion R1 rover.
* @min 1
* @group Roboclaw driver
*/
PARAM_DEFINE_INT32(RBCLW_COUNTS_REV, 1200);
/**
* Roboclaw serial baud rate
*
* Baud rate of the serial communication with the Roboclaw. The Roboclaw must be configured to match this rate.
* @min 1
* @max 8
* @value 1 2400 baud
* @value 2 9600 baud
* @value 3 19200 baud
* @value 4 38400 baud
* @value 5 57600 baud
* @value 6 115200 baud
* @value 7 230400 baud
* @value 8 460800 baud
* @group Roboclaw driver
*/
PARAM_DEFINE_INT32(RBCLW_BAUD, 8);
/**
* Communication retries
*
* If communication ever fails with the Roboclaw, it will be immediately retried, up to RBCLW_RETRIES times in total.
* @min 1
* @max 10
* @group Roboclaw driver
*/
PARAM_DEFINE_INT32(RBCLW_RETRIES, 1);
/**
* Stop retries
*
* When disarmed, if communication is interrupted with the Roboclaw, it will continue to try to stop up to
* this many times.
* @min 1
* @max 100
* @group Roboclaw driver
*/
PARAM_DEFINE_INT32(RBCLW_STOP_RETRY, 10);
/**
* Address of the Roboclaw
*
* The Roboclaw can be configured to have an address from 0x80 to 0x87, inclusive. It must be configured to match
* this parameter.
* @min 128
* @max 135
* @value 128 0x80
* @value 129 0x81
* @value 130 0x82
* @value 131 0x83
* @value 132 0x84
* @value 133 0x85
* @value 134 0x86
* @value 135 0x87
* @group Roboclaw driver
*/
PARAM_DEFINE_INT32(RBCLW_ADDRESS, 128);