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

drivers: remove RoboClaw

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-03-03 10:49:12 +01:00
committed by Daniel Agar
parent e5d5fcd315
commit 0633d0d826
8 changed files with 0 additions and 1238 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover
-8
View File
@@ -54,14 +54,6 @@ param set-default CBRK_AIRSPD_CHK 162128
# Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians
param set-default GND_MAX_ANG 3.1415
param set-default RBCLW_BAUD 8
param set-default RBCLW_COUNTS_REV 1200
param set-default RBCLW_ADDRESS 128
# 104 corresponds to Telem 4
param set-default RBCLW_SER_CFG 104
# Start this driver after setting parameters, because the driver uses some of those parameters.
# roboclaw start /dev/ttyS3
# Set geometry & output configration
param set-default CA_AIRFRAME 6
param set-default CA_R_REV 3
src/drivers/roboclaw/CMakeLists.txt
-43
View File
@@ -1,43 +0,0 @@
############################################################################
#
# Copyright (c) 2015 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.
#
############################################################################
px4_add_module(
MODULE drivers__roboclaw
MAIN roboclaw
COMPILE_FLAGS
SRCS
roboclaw_main.cpp
RoboClaw.cpp
MODULE_CONFIG
module.yaml
)
src/drivers/roboclaw/Kconfig
-5
View File
@@ -1,5 +0,0 @@
menuconfig DRIVERS_ROBOCLAW
bool "roboclaw"
default n
---help---
Enable support for roboclaw
src/drivers/roboclaw/RoboClaw.cpp
-610
View File
File diff suppressed because it is too large Load Diff
src/drivers/roboclaw/RoboClaw.hpp
-245
View File
@@ -1,245 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2013 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 RoboClas.hpp
*
* RoboClaw Motor Driver
*
* references:
* http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
*
*/
#pragma once
#include <poll.h>
#include <stdio.h>
#include <termios.h>
#include <lib/parameters/param.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/wheel_encoders.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/device/i2c.h>
#include <sys/select.h>
#include <sys/time.h>
#include <pthread.h>
/**
* This is a driver for the RoboClaw motor controller
*/
class RoboClaw
{
public:
void taskMain();
static bool taskShouldExit;
/** control channels */
enum e_channel {
CH_VOLTAGE_LEFT = 0,
CH_VOLTAGE_RIGHT
};
/** motors */
enum e_motor {
MOTOR_1 = 0,
MOTOR_2
};
/**
* 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
*/
virtual ~RoboClaw();
/**
* @return position of a motor, rev
*/
float getMotorPosition(e_motor motor);
/**
* @return speed of a motor, rev/sec
*/
float getMotorSpeed(e_motor motor);
/**
* set the speed of a motor, rev/sec
*/
int setMotorSpeed(e_motor motor, float value);
/**
* set the duty cycle of a motor
*/
int setMotorDutyCycle(e_motor motor, float value);
/**
* Drive both motors. +1 = full forward, -1 = full backward
*/
int drive(float value);
/**
* Turn. +1 = full right, -1 = full left
*/
int turn(float value);
/**
* reset the encoders
* @return status
*/
int resetEncoders();
/**
* read data from serial
*/
int readEncoder();
/**
* print status
*/
void printStatus(char *string, size_t n);
private:
// 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
};
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{};
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{};
int _uart;
fd_set _uart_set;
struct timeval _uart_timeout;
/** 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::PublicationMulti<wheel_encoders_s> _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};
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);
int _sendNothing(e_command);
/**
* 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);
};
src/drivers/roboclaw/module.yaml
-6
View File
@@ -1,6 +0,0 @@
module_name: Roboclaw Driver
serial_config:
- command: roboclaw start ${SERIAL_DEV} ${BAUD_PARAM}
port_config_param:
name: RBCLW_SER_CFG
group: Roboclaw
src/drivers/roboclaw/roboclaw_main.cpp
-207
View File
@@ -1,207 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2013 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_main.cpp
*
* RoboClaw Motor Driver
*
* references:
* http://downloads.ionmc.com/docs/roboclaw_user_manual.pdf
*
*/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/module.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <parameters/param.h>
#include "RoboClaw.hpp"
static bool thread_running = false; /**< Deamon status flag */
px4_task_t deamon_task;
/**
* Deamon management function.
*/
extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]);
/**
* Mainloop of deamon.
*/
int roboclaw_thread_main(int argc, char *argv[]);
/**
* Print the correct usage.
*/
static void usage();
static void usage()
{
PRINT_MODULE_USAGE_NAME("roboclaw", "driver");
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).
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
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,
use the `UART & I2C B` port, which corresponds to `/dev/ttyS3`.
### Implementation
The main loop of this module (Located in `RoboClaw.cpp::task_main()`) performs 2 tasks:
1. Write `actuator_controls_0` messages to the Roboclaw as they become available
2. Read encoder data from the Roboclaw at a constant, fixed rate.
Because of the latency of UART, this driver does not write every single `actuator_controls_0` message to the Roboclaw
immediately. Instead, it is rate limited based on the parameter `RBCLW_WRITE_PER`.
On startup, this driver will attempt to read the status of the Roboclaw to verify that it is connected. If this fails,
the driver terminates immediately.
### Examples
The command to start this driver is:
```
$ roboclaw start <device> <baud>
```
- `<device>` is the name of the UART port. On the Pixhawk 4, this is `/dev/ttyS3`.
- `<baud>` is the baud rate.
All available commands are:
- `$ roboclaw start <device> <baud>`
- `$ roboclaw status`
- `$ roboclaw stop`
)DESCR_STR");
}
/**
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
*
* The actual stack size should be set in the call
* to task_create().
*/
int roboclaw_main(int argc, char *argv[])
{
if (argc < 4) {
usage();
}
if (!strcmp(argv[1], "start")) {
if (thread_running) {
printf("roboclaw already running\n");
/* this is not an error */
return 0;
}
RoboClaw::taskShouldExit = false;
deamon_task = px4_task_spawn_cmd("roboclaw",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 10,
2000,
roboclaw_thread_main,
(char *const *)argv);
return 0;
} else if (!strcmp(argv[1], "stop")) {
RoboClaw::taskShouldExit = true;
return 0;
} else if (!strcmp(argv[1], "status")) {
if (thread_running) {
printf("\troboclaw app is running\n");
} else {
printf("\troboclaw app not started\n");
}
return 0;
}
usage();
return 1;
}
int roboclaw_thread_main(int argc, char *argv[])
{
printf("[roboclaw] starting\n");
// skip parent process args
argc -= 2;
argv += 2;
if (argc < 2) {
printf("usage: roboclaw start <device> <baud>\n");
return -1;
}
const char *deviceName = argv[1];
const char *baudRate = argv[2];
// start
RoboClaw roboclaw(deviceName, baudRate);
thread_running = true;
roboclaw.taskMain();
// exit
printf("[roboclaw] exiting.\n");
thread_running = false;
return 0;
}
src/drivers/roboclaw/roboclaw_params.c
-114
View File
@@ -1,114 +0,0 @@
/****************************************************************************
*
* 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);
/**
* 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);
/**
* Roboclaw serial baud rate
*
* Baud rate of the serial communication with the Roboclaw. The Roboclaw must be configured to match this rate.
* @min 2400
* @max 460800
* @value 2400 2400 baud
* @value 9600 9600 baud
* @value 19200 19200 baud
* @value 38400 38400 baud
* @value 57600 57600 baud
* @value 115200 115200 baud
* @value 230400 230400 baud
* @value 460800 460800 baud
* @group Roboclaw driver
* @reboot_required true
*/
PARAM_DEFINE_INT32(RBCLW_BAUD, 2400);