RoboClaw: declutter, make it compile again

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

msg/CMakeLists.txt

@@ -234,8 +234,8 @@ set(msg_files
 	VehicleTrajectoryBezier.msg
 	VehicleTrajectoryWaypoint.msg
 	VtolVehicleStatus.msg
-	Wind.msg
 	WheelEncoders.msg
+	Wind.msg
 	YawEstimatorStatus.msg
 )
 list(SORT msg_files)

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.
-

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

src/drivers/roboclaw/Kconfig

@@ -1,5 +1,5 @@
 menuconfig DRIVERS_ROBOCLAW
-	bool "crsf_rc"
+	bool "roboclaw"
 	default n
 	---help---
 		Enable support for roboclaw

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
+		// Read from topic to clear updated flag
-		parameter_update_s pupdate;
+		parameter_update_s parameter_update;
-		_parameter_update_sub.copy(&pupdate);
+		_parameter_update_sub.copy(&parameter_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 <UART device> <baud rate>`
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("roboclaw", "driver");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 	return 0;
 }
 

src/drivers/roboclaw/RoboClaw.hpp

@@ -36,41 +36,23 @@
  *
  * RoboClaw Motor Driver
  *
- * references:
+ * Product page: https://www.basicmicro.com/motor-controller
- * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
+ * Manual: https://downloads.basicmicro.com/docs/roboclaw_user_manual.pdf
- *
  */
 
 #pragma once
 
-#include <poll.h>
+#include <lib/mixer_module/mixer_module.hpp>
-#include <stdio.h>
-#include <termios.h>
-#include <lib/parameters/param.h>
-#include <drivers/device/i2c.h>
-#include <sys/select.h>
-#include <sys/time.h>
-#include <pthread.h>
-#include <unistd.h>
-
-#include <uORB/Subscription.hpp>
-
-#include <uORB/PublicationMulti.hpp>
-#include <uORB/topics/wheel_encoders.h>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_status.h>
-
-#include <uORB/topics/differential_drive_control.h>
-
-#include <px4_platform_common/px4_config.h>
-#include <px4_platform_common/defines.h>
 #include <px4_platform_common/module.h>
 #include <px4_platform_common/module_params.h>
-#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <sys/select.h>
 
-#include <lib/mixer_module/mixer_module.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
 
+#include <uORB/Publication.hpp>
+#include <uORB/topics/wheel_encoders.h>
 
 /**
  * This is a driver for the RoboClaw motor controller
@@ -216,11 +198,8 @@ private:
 	struct timeval _uart_fd_timeout;
 
 	uORB::SubscriptionData<actuator_armed_s> _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_s> _wheel_encoders_pub {ORB_ID(wheel_encoders)};

src/drivers/roboclaw/RoboclawSerialDevice.hpp

@@ -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 <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")};
-};

src/drivers/roboclaw/roboclaw_main.cpp

@@ -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 <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](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 <device> <baud>
-
-`<device>` is the name of the UART port. On the Pixhawk 4, this is `/dev/ttyS3`.
-`<baud>` is te 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 (!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 <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

@@ -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 <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
  *
@@ -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);