Implemented reading speed from the Roboclaw

msg/wheel_encoders.msg

@@ -9,7 +9,7 @@ uint64 timestamp			# time since system start (microseconds)
 # TODO: How large should the arrays be? What if we have a 6-wheeled rover?
 bool[4] has_encoder         # True for each wheel that has an encoder
 int64[4] encoder_position   # The wheel position, in encoder counts since boot. Positive is forward rotation, negative is reverse rotation
-float32[4] speed              # Speed of each wheel, in encoder counts per second. Positive is forward, negative is reverse
+int32[4] speed              # Speed of each wheel, in encoder counts per second. Positive is forward, negative is reverse
 
 # TODO: Should this be just one uint32, assuming each wheel has the same encoder?
 uint32[4] pulses_per_rev    # Number of pulses per revolution for each wheel

src/drivers/roboclaw/RoboClaw.cpp

@@ -209,13 +209,19 @@ void RoboClaw::taskMain()
 
 			if (readEncoder() > 0) {
 				_wheelEncoderMsg.timestamp = encoderTaskLastRun;
+
 				_wheelEncoderMsg.encoder_position[0] = _encoderCounts[0];
 				_wheelEncoderMsg.encoder_position[1] = _encoderCounts[1];
 
-				//PX4_INFO("[%llu] PUBLISHING", _wheelEncoderMsg.timestamp);
-				orb_publish(ORB_ID(wheel_encoders), _wheelEncodersAdv, &_wheelEncoderMsg);
+				_wheelEncoderMsg.speed[0] = _motorSpeeds[0];
+				_wheelEncoderMsg.speed[1] = _motorSpeeds[1];
+
+				if (_wheelEncodersAdv == nullptr) {
+					_wheelEncodersAdv = orb_advertise(ORB_ID(wheel_encoders), &_wheelEncoderMsg);
 
-				//PX4_INFO("[%llu] Reading encoders", hrt_absolute_time());
+				} else {
+					orb_publish(ORB_ID(wheel_encoders), _wheelEncodersAdv, &_wheelEncoderMsg);
+				}
 
 			} else {
 				PX4_ERR("Error reading encoders");
@@ -229,35 +235,45 @@ void RoboClaw::taskMain()
 	orb_unsubscribe(_actuatorsSub);
 	orb_unsubscribe(_armedSub);
 	orb_unsubscribe(_paramSub);
-	orb_unadvertise(_wheelEncodersAdv);
 }
 
 int RoboClaw::readEncoder()
 {
 
-	uint8_t rbuff[ENCODER_MESSAGE_SIZE];
-	int nread = 0;
+	uint8_t rbuff_pos[ENCODER_MESSAGE_SIZE];
+	uint8_t rbuff_speed[11];
 
-	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);
+	bool success = false;
+
+	for (int retry = 0; retry < _parameters.serial_timeout_retries && !success; retry++) {
+		success = true;
+		success &=  _transaction(CMD_READ_BOTH_ENCODERS, nullptr, 0, &rbuff_pos[0], ENCODER_MESSAGE_SIZE, false,
+					 true) == ENCODER_MESSAGE_SIZE;
+		success &=  _transaction(CMD_READ_SPEED_1, nullptr, 0, &rbuff_speed[0], 7, false, true) == 7;
+		success &=  _transaction(CMD_READ_SPEED_2, nullptr, 0, &rbuff_speed[4], 7, false, true) == 7;
 	}
 
-	if (nread < ENCODER_MESSAGE_SIZE) {
-		PX4_ERR("Error reading encoders: %d", nread);
+	if (!success) {
+		PX4_ERR("Error reading encoders");
 		return -1;
 	}
 
 	uint32_t count;
+	uint32_t speed;
 	uint8_t *count_bytes;
+	uint8_t *speed_bytes;
 
 	for (int motor = 0; motor <= 1; motor++) {
 		count = 0;
-		count_bytes = &rbuff[motor * 4];
+		speed = 0;
+		count_bytes = &rbuff_pos[motor * 4];
+		speed_bytes = &rbuff_speed[motor * 4];
 
 		// Data from the roboclaw is big-endian. This converts the bytes to an integer, regardless of the
 		// endianness of the system this code is running on.
 		for (int byte = 0; byte < 4; byte++) {
 			count = (count << 8) + count_bytes[byte];
+			speed = (speed << 8) + speed_bytes[byte];
 		}
 
 		// The Roboclaw stores encoder counts as unsigned 32-bit ints. This can overflow, especially when starting
@@ -275,9 +291,9 @@ int RoboClaw::readEncoder()
 		// At this point, abs(diff) is always <= 2^31, so this cast from unsigned to signed is safe.
 		int32_t diff = fwd_diff <= rev_diff ? fwd_diff : -int32_t(rev_diff);
 		_encoderCounts[motor] += diff;
-//		PX4_INFO("Motor %d - LastCount: %7u, Count: %7u, Diff1: %8u, Diff2: %8u, diff: %4d, End counts: %4lld",
-//			 motor, _lastEncoderCount[motor], count, fwd_diff, rev_diff, diff, _encoderCounts[motor]);
 		_lastEncoderCount[motor] = count;
+
+		_motorSpeeds[motor] = speed;
 	}
 
 	return 1;
@@ -517,7 +533,6 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
 			return bytes_read;
 
 		} else {
-			//PX4_ERR("Invalid checksum. Expected 0x%04X, got 0x%04X", checksum_calc, checksum_recv);
 			return -10;
 		}
 

src/drivers/roboclaw/RoboClaw.hpp

@@ -160,8 +160,11 @@ private:
 		// 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,

src/drivers/roboclaw/roboclaw_main.cpp

@@ -45,6 +45,7 @@
 
 #include <px4_config.h>
 #include <px4_log.h>
+#include <px4_module.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <stdlib.h>
@@ -57,7 +58,7 @@
 #include "RoboClaw.hpp"
 
 static bool thread_running = false;     /**< Deamon status flag */
-static int deamon_task;             /**< Handle of deamon task / thread */
+px4_task_t deamon_task;
 
 /**
  * Deamon management function.
@@ -76,7 +77,48 @@ static void usage();
 
 static void usage()
 {
-	PX4_INFO("usage: roboclaw {start|stop|status|test}");
+	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.ionmc.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>
+
+`<device>` is the name of the UART port. On the Pixhawk 4, this is `/dev/ttyS3`.
+
+All available commands are:
+
+ - `$ roboclaw start <device>`
+ - `$ roboclaw status`
+ - `$ roboclaw stop`
+	)DESCR_STR");
 }
 
 /**