From db950df94161b0b6e436a88a963338f543358273 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 13 Nov 2023 17:51:58 +0100 Subject: [PATCH] RoboClaw: declutter, make it compile again --- boards/px4/fmu-v5x/default.px4board | 2 +- msg/CMakeLists.txt | 2 +- msg/WheelEncoders.msg | 1 - src/drivers/roboclaw/CMakeLists.txt | 9 +- src/drivers/roboclaw/Kconfig | 2 +- src/drivers/roboclaw/RoboClaw.cpp | 34 ++- src/drivers/roboclaw/RoboClaw.hpp | 35 +-- src/drivers/roboclaw/RoboclawSerialDevice.hpp | 66 ------ src/drivers/roboclaw/roboclaw_main.cpp | 200 ------------------ src/drivers/roboclaw/roboclaw_params.c | 55 +---- 10 files changed, 40 insertions(+), 366 deletions(-) delete mode 100644 src/drivers/roboclaw/RoboclawSerialDevice.hpp delete mode 100644 src/drivers/roboclaw/roboclaw_main.cpp diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index fc0d33709506..9ec645eca72a 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -15,7 +15,7 @@ CONFIG_DRIVERS_BATT_SMBUS=y CONFIG_DRIVERS_CAMERA_CAPTURE=y CONFIG_DRIVERS_CAMERA_TRIGGER=y CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y -CONFIG_COMMON_DISTANCE_SENSOR=n +CONFIG_COMMON_DISTANCE_SENSOR=y CONFIG_DRIVERS_DSHOT=y CONFIG_DRIVERS_ROBOCLAW=y CONFIG_DRIVERS_GPIO_MCP23009=y diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 49180e905da0..8570b4aa3cfc 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -226,8 +226,8 @@ set(msg_files VehicleTrajectoryBezier.msg VehicleTrajectoryWaypoint.msg VtolVehicleStatus.msg - Wind.msg WheelEncoders.msg + Wind.msg YawEstimatorStatus.msg ) list(SORT msg_files) diff --git a/msg/WheelEncoders.msg b/msg/WheelEncoders.msg index dfeeeca4322c..368aa9360bad 100644 --- a/msg/WheelEncoders.msg +++ b/msg/WheelEncoders.msg @@ -2,4 +2,3 @@ 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. - diff --git a/src/drivers/roboclaw/CMakeLists.txt b/src/drivers/roboclaw/CMakeLists.txt index daad2f40b97c..ee2570a68742 100644 --- a/src/drivers/roboclaw/CMakeLists.txt +++ b/src/drivers/roboclaw/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# Copyright (c) 2015-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 @@ -31,18 +31,11 @@ # ############################################################################ -set(PARAM_PREFIX ROBOCLAW) - -if(CONFIG_BOARD_IO) - set(PARAM_PREFIX ROBOCLAW) -endif() - px4_add_module( MODULE drivers__roboclaw MAIN roboclaw COMPILE_FLAGS SRCS - roboclaw_main.cpp RoboClaw.cpp MODULE_CONFIG module.yaml diff --git a/src/drivers/roboclaw/Kconfig b/src/drivers/roboclaw/Kconfig index 696c14023346..93395269a8d1 100644 --- a/src/drivers/roboclaw/Kconfig +++ b/src/drivers/roboclaw/Kconfig @@ -1,5 +1,5 @@ menuconfig DRIVERS_ROBOCLAW - bool "crsf_rc" + bool "roboclaw" default n ---help--- Enable support for roboclaw diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 35533d8ee7f1..9dafe6859218 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -78,9 +78,7 @@ using namespace time_literals; 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 @@ -251,9 +249,9 @@ void RoboClaw::Run() // check for parameter updates if (_parameter_update_sub.updated()) { - // clear update - parameter_update_s pupdate; - _parameter_update_sub.copy(&pupdate); + // Read from topic to clear updated flag + parameter_update_s parameter_update; + _parameter_update_sub.copy(¶meter_update); updateParams(); } @@ -530,11 +528,35 @@ RoboClaw::RoboClawError RoboClaw::writeCommandWithPayload(e_command cmd, uint8_t int RoboClaw::custom_command(int argc, char *argv[]) { - return 0; + return print_usage("unknown command"); } int RoboClaw::print_usage(const char *reason) { + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION(R"DESCR_STR( +### Description + +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 OutputModuleInterface. + - 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. +The driver needs to be enabled using the parameter `RBCLW_SER_CFG`, the baudrate needs to be set correctly and +the address `RBCLW_ADDRESS` needs to match the ESC configuration. + +The command to start this driver is: `$ roboclaw start ` +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("roboclaw", "driver"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; } diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp index 381deb08aaec..008332ef1390 100644 --- a/src/drivers/roboclaw/RoboClaw.hpp +++ b/src/drivers/roboclaw/RoboClaw.hpp @@ -36,41 +36,23 @@ * * RoboClaw Motor Driver * - * references: - * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf - * + * Product page: https://www.basicmicro.com/motor-controller + * Manual: https://downloads.basicmicro.com/docs/roboclaw_user_manual.pdf */ #pragma once -#include -#include -#include -#include -#include +#include +#include +#include #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 @@ -216,11 +198,8 @@ class RoboClaw : public ModuleBase, public OutputModuleInterface struct timeval _uart_fd_timeout; 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)}; - differential_drive_control_s _diff_drive_control{}; - matrix::Vector2f _motor_control{0.0f, 0.0f}; uORB::Publication _wheel_encoders_pub {ORB_ID(wheel_encoders)}; diff --git a/src/drivers/roboclaw/RoboclawSerialDevice.hpp b/src/drivers/roboclaw/RoboclawSerialDevice.hpp deleted file mode 100644 index f70f3c5385b3..000000000000 --- a/src/drivers/roboclaw/RoboclawSerialDevice.hpp +++ /dev/null @@ -1,66 +0,0 @@ -/**************************************************************************** - * - * 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 - */ - -#include "RoboclawDriver/RoboclawDriver.hpp" -#include - -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")}; -}; diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp deleted file mode 100644 index a3483c25a548..000000000000 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ /dev/null @@ -1,200 +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 -#include -#include -#include -#include -#include -#include -#include - -#include - - -#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](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 - -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 - -`` is the name of the UART port. On the Pixhawk 4, this is `/dev/ttyS3`. -`` is te baud rate. - -All available commands are: - - - `$ roboclaw start ` - - `$ 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 (!strcmp(argv[1], "start") && (argc >= 4)) { - - 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 \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; -} diff --git a/src/drivers/roboclaw/roboclaw_params.c b/src/drivers/roboclaw/roboclaw_params.c index 849d95663812..938e8530359a 100644 --- a/src/drivers/roboclaw/roboclaw_params.c +++ b/src/drivers/roboclaw/roboclaw_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2019 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 @@ -31,40 +31,6 @@ * ****************************************************************************/ -/** - * @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 - */ - - -/** - * 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 * @@ -93,22 +59,3 @@ PARAM_DEFINE_INT32(RBCLW_COUNTS_REV, 1200); * @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, 57600);