Changed wheel encoder publication to multi-instance

msg/wheel_encoders.msg

@@ -1,15 +1,5 @@
-# TODO: How should this mapping be done? What if there's a 6-wheeled (or more) rover?
-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
-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
+int64 encoder_position   # The wheel position, in encoder counts since boot. Positive is forward rotation, negative is reverse rotation
+int32 speed              # Speed of each wheel, in encoder counts per second. Positive is forward, negative is reverse
+uint32 pulses_per_rev    # Number of pulses per revolution for each wheel

src/drivers/roboclaw/RoboClaw.cpp

@@ -170,9 +170,9 @@ void RoboClaw::taskMain()
 	fds[2].fd = _armedSub;
 	fds[2].events = POLLIN;
 
-	memset((void *) &_wheelEncoderMsg, 0, sizeof(wheel_encoders_s));
-	_wheelEncoderMsg.timestamp = hrt_absolute_time();
-	_wheelEncodersAdv = orb_advertise(ORB_ID(wheel_encoders), &_wheelEncoderMsg);
+	memset((void *) &_wheelEncoderMsg[0], 0, sizeof(_wheelEncoderMsg));
+	_wheelEncoderMsg[0].pulses_per_rev = _parameters.counts_per_rev;
+	_wheelEncoderMsg[1].pulses_per_rev = _parameters.counts_per_rev;
 
 	while (!taskShouldExit) {
 
@@ -221,19 +221,21 @@ void RoboClaw::taskMain()
 			encoderTaskLastRun = hrt_absolute_time();
 
 			if (readEncoder() > 0) {
-				_wheelEncoderMsg.timestamp = encoderTaskLastRun;
 
-				_wheelEncoderMsg.encoder_position[0] = _encoderCounts[0];
-				_wheelEncoderMsg.encoder_position[1] = _encoderCounts[1];
+				for (int i = 0; i < 2; i++) {
+					_wheelEncoderMsg[i].timestamp = encoderTaskLastRun;
 
-				_wheelEncoderMsg.speed[0] = _motorSpeeds[0];
-				_wheelEncoderMsg.speed[1] = _motorSpeeds[1];
+					_wheelEncoderMsg[i].encoder_position = _encoderCounts[i];
+					_wheelEncoderMsg[i].speed = _motorSpeeds[i];
 
-				if (_wheelEncodersAdv == nullptr) {
-					_wheelEncodersAdv = orb_advertise(ORB_ID(wheel_encoders), &_wheelEncoderMsg);
+					if (_wheelEncodersAdv[i] == nullptr) {
+						int instance;
+						_wheelEncodersAdv[i] = orb_advertise_multi(ORB_ID(wheel_encoders), &_wheelEncoderMsg[i],
+								       &instance, ORB_PRIO_DEFAULT);
 
 					} else {
-					orb_publish(ORB_ID(wheel_encoders), _wheelEncodersAdv, &_wheelEncoderMsg);
+						orb_publish(ORB_ID(wheel_encoders), _wheelEncodersAdv[i], &_wheelEncoderMsg[i]);
+					}
 				}
 
 			} else {

src/drivers/roboclaw/RoboClaw.hpp

@@ -195,21 +195,21 @@ private:
 	struct timeval _uart_timeout;
 
 	/** actuator controls subscription */
-	int _actuatorsSub;
+	int _actuatorsSub{-1};
 	actuator_controls_s _actuatorControls;
 
-	int _armedSub;
+	int _armedSub{-1};
 	actuator_armed_s _actuatorArmed;
 
-	int _paramSub;
+	int _paramSub{-1};
 	parameter_update_s _paramUpdate;
 
-	orb_advert_t _wheelEncodersAdv;
-	wheel_encoders_s _wheelEncoderMsg;
+	orb_advert_t _wheelEncodersAdv[2] {nullptr, nullptr};
+	wheel_encoders_s _wheelEncoderMsg[2];
 
-	uint32_t _lastEncoderCount[2];
-	int64_t _encoderCounts[2];
-	int32_t _motorSpeeds[2];
+	uint32_t _lastEncoderCount[2] {0, 0};
+	int64_t _encoderCounts[2] {0, 0};
+	int32_t _motorSpeeds[2] {0, 0};
 
 	void _parameters_update();