Publishing encoder message at regular intervals

msg/CMakeLists.txt

@@ -142,6 +142,7 @@ set(msg_files
 	vehicle_status_flags.msg
 	vehicle_trajectory_waypoint.msg
 	vtol_vehicle_status.msg
+	wheel_encoders.msg
 	wind_estimate.msg
 	)
 

msg/wheel_encoders.msg

@@ -1,15 +1,15 @@
 # TODO: How should this mapping be done? What if there's a 6-wheeled (or more) rover?
-uint8_t FRONT_RIGHT = 0
-uint8_t FRONT_LEFT = 1
-uint8_t REAR_RIGHT = 2
-uint8_t REAR_LEFT = 3
+uint8 FRONT_RIGHT = 0
+uint8 FRONT_LEFT = 1
+uint8 REAR_RIGHT = 2
+uint8 REAR_LEFT = 3
 
 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
-float[4] speed              # Speed of each wheel, in encoder counts per second. Positive is forward, negative is reverse
+float32[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

@@ -61,11 +61,10 @@
 // The RoboClaw has a serial communication timeout of 10ms.
 #define TIMEOUT_US 10000
 
-// TODO: Make this a parameter
+// TODO: Make these all parameters
 #define FAILED_TRANSACTION_RETRIES 1
-
-// The RoboClaw determines the change in the wheel encoder value when it overflows
-#define OVERFLOW_AMOUNT 0x100000000LL
+#define ENCODER_READ_PERIOD_MS 10
+#define ACTUATOR_WRITE_PERIOD_MS 10
 
 // TODO: Delete this
 //void printbytes(const char *msg, uint8_t *bytes, int numbytes)
@@ -86,24 +85,21 @@
 //	PX4_INFO("%s", buff);
 //}
 
+bool RoboClaw::taskShouldExit = false;
+
 RoboClaw::RoboClaw(const char *deviceName, uint16_t address, uint16_t pulsesPerRev):
-	ScheduledWorkItem(px4::wq_configurations::hp_default),
 	_address(address),
 	_pulsesPerRev(pulsesPerRev),
 	_uart(0),
+	_uart_set(),
 	_uart_timeout{.tv_sec = 0, .tv_usec = TIMEOUT_US},
-	_uart_mutex(PTHREAD_MUTEX_INITIALIZER),
-	_controlPoll(),
-	_actuators(ORB_ID(actuator_controls_0), 20),
+	_actuatorsOrbID(ORB_ID(actuator_controls_0)),
+	_wheelEncodersOrbID(ORB_ID(wheel_encoders)),
 	_lastEncoderCount{0, 0},
 	_encoderCounts{0, 0},
 	_motorSpeeds{0, 0}
 
 {
-	// setup control polling
-	_controlPoll.fd = _actuators.getHandle();
-	_controlPoll.events = POLLIN;
-
 	// start serial port
 	_uart = open(deviceName, O_RDWR | O_NOCTTY);
 
@@ -130,8 +126,6 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address, uint16_t pulsesPerR
 
 	FD_ZERO(&_uart_set);
 
-	pthread_mutex_init(&_uart_mutex, nullptr);
-
 	// setup default settings, reset encoders
 	resetEncoders();
 }
@@ -141,17 +135,62 @@ RoboClaw::~RoboClaw()
 	setMotorDutyCycle(MOTOR_1, 0.0);
 	setMotorDutyCycle(MOTOR_2, 0.0);
 	close(_uart);
-
-	pthread_mutex_destroy(&_uart_mutex);
 }
 
-void RoboClaw::Run()
+void RoboClaw::taskMain()
 {
-	readEncoder();
-	//readEncoder(MOTOR_2);
+	uint64_t encoderTaskLastRun = 0;
+	int waitTime = 0;
+
+	_actuatorsSub = orb_subscribe(_actuatorsOrbID);
+	orb_set_interval(_actuatorsSub, ACTUATOR_WRITE_PERIOD_MS);
+	_actuatorsPoll.fd = _actuatorsSub;
+	_actuatorsPoll.events = POLLIN;
+
+	memset((void *) &_wheelEncoderMsg, 0, sizeof(wheel_encoders_s));
+	_wheelEncoderMsg.timestamp = hrt_absolute_time();
+	_wheelEncodersAdv = orb_advertise(_wheelEncodersOrbID, &_wheelEncoderMsg);
+
+	while (!taskShouldExit) {
+
+		int pret = poll(&_actuatorsPoll, 1, waitTime / 1000);
+
+		if (pret > 0 && _actuatorsPoll.revents & POLLIN) {
+			orb_copy(_actuatorsOrbID, _actuatorsSub, &_actuatorControls);
+			int drive_ret = drive(_actuatorControls.control[actuator_controls_s::INDEX_THROTTLE]);
+			int turn_ret = turn(_actuatorControls.control[actuator_controls_s::INDEX_YAW]);
+
+			if (drive_ret <= 0 || turn_ret <= 0) {
+				PX4_ERR("Error controlling RoboClaw. Drive err: %d. Turn err: %d", drive_ret, turn_ret);
+			}
+
+			//PX4_INFO("[%llu] Writing actuators", hrt_absolute_time());
+
+		} else {
+			encoderTaskLastRun = hrt_absolute_time();
 
-	//PX4_INFO("Motor1: (%d, %d), Motor2: (%d, %d)", _motor1EncoderCounts, _motor1Revolutions, _motor2EncoderCounts,
-	//	 _motor2Revolutions);
+			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);
+
+				//PX4_INFO("[%llu] Reading encoders", hrt_absolute_time());
+
+			} else {
+				PX4_ERR("Error reading encoders");
+			}
+		}
+
+		waitTime = ENCODER_READ_PERIOD_MS * 1000 - (hrt_absolute_time() - encoderTaskLastRun);
+		waitTime = waitTime < 0 ? 0 : waitTime;
+		//PX4_INFO("ROBOCLAW WAIT TIME: %d", waitTime);
+	}
+
+	orb_unsubscribe(_actuatorsSub);
+	orb_unadvertise(_wheelEncodersAdv);
 }
 
 int RoboClaw::readEncoder()
@@ -306,33 +345,31 @@ int RoboClaw::resetEncoders()
 	return _sendNothing(CMD_RESET_ENCODERS);
 }
 
-int RoboClaw::update()
-{
-	//TODO: Also update motor locations and speeds here
-
-	// wait for an actuator publication,
-	// check for exit condition every second
-	// note "::poll" is required to distinguish global
-	// poll from member function for driver
-	if (::poll(&_controlPoll, 1, 1000) < 0) { return -1; } // poll error
-
-	// if new data, send to motors
-	if (_actuators.updated()) {
-		_actuators.update();
-		// setMotorDutyCycle(MOTOR_1, _actuators.get().control[actuator_controls_s::INDEX_]);
-		// setMotorDutyCycle(MOTOR_2, _actuators.get().control[CH_VOLTAGE_RIGHT]);
-		int drive_ret = drive(_actuators.get().control[actuator_controls_s::INDEX_THROTTLE]);
-		int turn_ret = turn(_actuators.get().control[actuator_controls_s::INDEX_YAW]);
-
-		if (drive_ret <= 0 || turn_ret <= 0) {
-			PX4_ERR("Error controlling RoboClaw. Drive err: %d. Turn err: %d", drive_ret, turn_ret);
-		}
-	}
-
-	Run();
-
-	return 0;
-}
+//int RoboClaw::update()
+//{
+//	//TODO: Also update motor locations and speeds here
+//
+//	// wait for an actuator publication,
+//	// check for exit condition every second
+//	// note "::poll" is required to distinguish global
+//	// poll from member function for driver
+//	if (::poll(&_controlPoll, 1, 1000) < 0) { return -1; } // poll error
+//
+//	// if new data, send to motors
+//	if (_actuators.updated()) {
+//		_actuators.update();
+//		int drive_ret = drive(_actuators.get().control[actuator_controls_s::INDEX_THROTTLE]);
+//		int turn_ret = turn(_actuators.get().control[actuator_controls_s::INDEX_YAW]);
+//
+//		if (drive_ret <= 0 || turn_ret <= 0) {
+//			PX4_ERR("Error controlling RoboClaw. Drive err: %d. Turn err: %d", drive_ret, turn_ret);
+//		}
+//	}
+//
+//	Run();
+//
+//	return 0;
+//}
 
 int RoboClaw::_sendUnsigned7Bit(e_command command, float data)
 {
@@ -390,9 +427,9 @@ uint16_t RoboClaw::_calcCRC(const uint8_t *buf, size_t n, uint16_t init)
 int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
 			   uint8_t *rbuff, size_t rbytes, bool send_checksum, bool recv_checksum)
 {
-	// WRITE
+	int err_code = 0;
 
-	pthread_mutex_lock(&_uart_mutex);
+	// WRITE
 
 	tcflush(_uart, TCIOFLUSH); // flush  buffers
 	uint8_t buf[wbytes + 4];
@@ -415,7 +452,6 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
 
 	if (count < (int) wbytes) { // Did not successfully send all bytes.
 		PX4_ERR("Only wrote %d out of %d bytes", count, (int) wbytes);
-		pthread_mutex_unlock(&_uart_mutex);
 		return -1;
 	}
 
@@ -424,52 +460,57 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
 	FD_ZERO(&_uart_set);
 	FD_SET(_uart, &_uart_set);
 
-	int rv = select(_uart + 1, &_uart_set, nullptr, nullptr, &_uart_timeout);
+	uint8_t *rbuff_curr = rbuff;
+	size_t bytes_read = 0;
 
-	//TODO: Clean up this mess of IFs and returns
+	// select(...) returns as soon as even 1 byte is available. read(...) returns immediately, no matter how many
+	// bytes are available. I need to keep reading until I get the number of bytes I expect.
+	while (bytes_read < rbytes) {
+		err_code = select(_uart + 1, &_uart_set, nullptr, nullptr, &_uart_timeout);
 
-	if (rv > 0) {
-		// select() returns as soon as ANY bytes are available. I need to wait until ALL of the bytes are available.
-		// TODO: Make sure this is not a busy wait.
-		usleep(2000);
-		int bytes_read = read(_uart, rbuff, rbytes);
+		if (err_code < 0) {
+			return err_code;
+		}
 
-		if (recv_checksum) {
-			if (bytes_read < 2) {
-				pthread_mutex_unlock(&_uart_mutex);
-				return -1;
-			}
+		err_code = read(_uart, rbuff_curr, rbytes - bytes_read);
+
+		if (err_code < 0) {
+			return err_code;
 
-			// The checksum sent back by the roboclaw is calculated based on the address and command bytes as well
-			// as the data returned.
-			uint16_t checksum_calc = _calcCRC(buf, 2);
-			checksum_calc = _calcCRC(rbuff, bytes_read - 2, checksum_calc);
-			uint16_t checksum_recv = (rbuff[bytes_read - 2] << 8) + rbuff[bytes_read - 1];
+		} else {
+			bytes_read += err_code;
+			rbuff_curr += err_code;
+		}
+	}
 
-			if (checksum_calc == checksum_recv) {
-				pthread_mutex_unlock(&_uart_mutex);
-				return bytes_read;
+	//TODO: Clean up this mess of IFs and returns
 
-			} else {
-				//PX4_ERR("Invalid checksum. Expected 0x%04X, got 0x%04X", checksum_calc, checksum_recv);
-				pthread_mutex_unlock(&_uart_mutex);
-				return -10;
-			}
+	if (recv_checksum) {
+		if (bytes_read < 2) {
+			return -1;
+		}
 
-		} else {
-			if (bytes_read == 1 && rbuff[0] == 0xFF) {
-				pthread_mutex_unlock(&_uart_mutex);
-				return 1;
+		// The checksum sent back by the roboclaw is calculated based on the address and command bytes as well
+		// as the data returned.
+		uint16_t checksum_calc = _calcCRC(buf, 2);
+		checksum_calc = _calcCRC(rbuff, bytes_read - 2, checksum_calc);
+		uint16_t checksum_recv = (rbuff[bytes_read - 2] << 8) + rbuff[bytes_read - 1];
 
-			} else {
-				pthread_mutex_unlock(&_uart_mutex);
-				return -11;
-			}
+		if (checksum_calc == checksum_recv) {
+			return bytes_read;
+
+		} else {
+			//PX4_ERR("Invalid checksum. Expected 0x%04X, got 0x%04X", checksum_calc, checksum_recv);
+			return -10;
 		}
 
 	} else {
-		pthread_mutex_unlock(&_uart_mutex);
-		return rv;
+		if (bytes_read == 1 && rbuff[0] == 0xFF) {
+			return 1;
+
+		} else {
+			return -11;
+		}
 	}
 }
 

src/drivers/roboclaw/RoboClaw.hpp

@@ -47,21 +47,22 @@
 #include <stdio.h>
 #include <uORB/SubscriptionPollable.hpp>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/wheel_encoders.h>
 #include <drivers/device/i2c.h>
 #include <sys/select.h>
 #include <sys/time.h>
 #include <pthread.h>
-//#include <px4.h>
-#include <px4_work_queue/ScheduledWorkItem.hpp>
 
 /**
  * This is a driver for the RoboClaw motor controller
  */
-class RoboClaw : public px4::ScheduledWorkItem
+class RoboClaw
 {
 public:
 
 	static int roboclawTest(int argc, char *argv[]);
+	void taskMain();
+	static bool taskShouldExit;
 
 	/** control channels */
 	enum e_channel {
@@ -190,19 +191,22 @@ class RoboClaw : public px4::ScheduledWorkItem
 	fd_set _uart_set;
 	struct timeval _uart_timeout;
 
-	pthread_mutex_t _uart_mutex;
-
 	/** poll structure for control packets */
-	struct pollfd _controlPoll;
+	struct pollfd _actuatorsPoll;
 
 	/** actuator controls subscription */
-	uORB::SubscriptionPollable<actuator_controls_s> _actuators;
+	int _actuatorsSub;
+	const struct orb_metadata *_actuatorsOrbID;
+	actuator_controls_s _actuatorControls;
+
+	orb_advert_t _wheelEncodersAdv;
+	const struct orb_metadata *_wheelEncodersOrbID;
+	wheel_encoders_s _wheelEncoderMsg;
 
 	uint32_t _lastEncoderCount[2];
 	int64_t _encoderCounts[2];
 	int32_t _motorSpeeds[2];
 
-
 	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);
@@ -211,8 +215,7 @@ class RoboClaw : public px4::ScheduledWorkItem
 	/**
 	 * Perform a round-trip write and read.
 	 *
-	 * NOTE: This function uses a mutex contained in this class. This makes it thread-safe, but also a potential
-	 * source of deadlock.
+	 * 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