Further fixing up parameters

ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover

@@ -17,8 +17,6 @@
 
 sh /etc/init.d/rc.rover_defaults
 
-roboclaw start /dev/ttyS3 128 1200
-
 if [ $AUTOCNF = yes ]
 then
 	param set BAT_N_CELLS 4
@@ -68,8 +66,18 @@ then
 
 	# Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor
 	param set CBRK_AIRSPD_CHK 162128
+
+	# Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians
+	param set GND_MAX_ANG 3.1415
+
+	param set RBCLW_BAUD 8
+	param set RBCLW_COUNTS_REV 1200
+	param set RBCLW_ADDRESS 128
 fi
 
+# Start this driver after setting parameters, because the driver uses some of those parameters.
+roboclaw start /dev/ttyS3
+
 # Configure this as rover
 set MAV_TYPE 10
 

src/drivers/roboclaw/RoboClaw.cpp

@@ -71,6 +71,7 @@ RoboClaw::RoboClaw(const char *deviceName):
 	_uart(0),
 	_uart_set(),
 	_uart_timeout{.tv_sec = 0, .tv_usec = TIMEOUT_US},
+	_actuatorsSub(-1),
 	_lastEncoderCount{0, 0},
 	_encoderCounts{0, 0},
 	_motorSpeeds{0, 0}
@@ -80,7 +81,7 @@ RoboClaw::RoboClaw(const char *deviceName):
 	_param_handles.actuator_write_period_ms = 	param_find("RBCLW_WRITE_PER");
 	_param_handles.encoder_read_period_ms = 	param_find("RBCLW_READ_PER");
 	_param_handles.counts_per_rev = 			param_find("RBCLW_COUNTS_REV");
-	_param_handles.serial_baud_rate = 			param_find("RMCLW_BAUD");
+	_param_handles.serial_baud_rate = 			param_find("RBCLW_BAUD");
 	_param_handles.serial_timeout_retries = 	param_find("RBCLW_RETRIES");
 	_param_handles.stop_retries = 				param_find("RBCLW_STOP_RETRY");
 	_param_handles.address = 					param_find("RBCLW_ADDRESS");
@@ -126,6 +127,14 @@ RoboClaw::~RoboClaw()
 
 void RoboClaw::taskMain()
 {
+	// Make sure the Roboclaw is actually connected, so I don't just spam errors if it's not.
+	uint8_t rbuff[4];
+	int err_code = _transaction(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true);
+
+	if (err_code <= 0) {
+		PX4_ERR("Unable to connect to Roboclaw. Shutting down Roboclaw driver.");
+		return;
+	}
 
 	// This main loop performs two different tasks, asynchronously:
 	// - Send actuator_controls_0 to the Roboclaw as soon as they are available
@@ -142,13 +151,15 @@ void RoboClaw::taskMain()
 	orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms);
 
 	_armedSub = orb_subscribe(ORB_ID(actuator_armed));
+	_paramSub = orb_subscribe(ORB_ID(parameter_update));
 
-	//TODO: Add parameter listening
-	pollfd fds[2];
-	fds[0].fd = _actuatorsSub;
+	pollfd fds[3];
+	fds[0].fd = _paramSub;
 	fds[0].events = POLLIN;
-	fds[1].fd = _armedSub;
+	fds[1].fd = _actuatorsSub;
 	fds[1].events = POLLIN;
+	fds[2].fd = _armedSub;
+	fds[2].events = POLLIN;
 
 	memset((void *) &_wheelEncoderMsg, 0, sizeof(wheel_encoders_s));
 	_wheelEncoderMsg.timestamp = hrt_absolute_time();
@@ -158,8 +169,13 @@ void RoboClaw::taskMain()
 
 		int pret = poll(fds, sizeof(fds) / sizeof(pollfd), waitTime / 1000);
 
+		if (fds[0].revents & POLLIN) {
+			orb_copy(ORB_ID(parameter_update), _paramSub, &_paramUpdate);
+			_parameters_update();
+		}
+
 		// No timeout, update on either the actuator controls or the armed state
-		if (pret > 0 && (fds[0].revents & POLLIN || fds[1].revents & POLLIN)) {
+		if (pret > 0 && (fds[1].revents & POLLIN || fds[2].revents & POLLIN)) {
 			orb_copy(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuatorControls);
 			orb_copy(ORB_ID(actuator_armed), _armedSub, &_actuatorArmed);
 
@@ -170,8 +186,11 @@ void RoboClaw::taskMain()
 
 			if (disarmed) {
 				// If disarmed, I want to be certain that the stop command gets through.
-				while ((drive_ret = drive(0.0)) <= 0 || (turn_ret = turn(0.0)) <= 0) {
-					//PX4_ERR("Error trying to stop: Drive: %d, Turn: %d", drive_ret, turn_ret);
+				int tries = 0;
+
+				while (tries < _parameters.stop_retries && ((drive_ret = drive(0.0)) <= 0 || (turn_ret = turn(0.0)) <= 0)) {
+					PX4_ERR("Error trying to stop: Drive: %d, Turn: %d", drive_ret, turn_ret);
+					tries++;
 					px4_usleep(TIMEOUT_US);
 				}
 
@@ -209,6 +228,7 @@ void RoboClaw::taskMain()
 
 	orb_unsubscribe(_actuatorsSub);
 	orb_unsubscribe(_armedSub);
+	orb_unsubscribe(_paramSub);
 	orb_unadvertise(_wheelEncodersAdv);
 }
 
@@ -520,6 +540,10 @@ void RoboClaw::_parameters_update()
 	param_get(_param_handles.actuator_write_period_ms, &_parameters.actuator_write_period_ms);
 	param_get(_param_handles.address, &_parameters.address);
 
+	if (_actuatorsSub > 0) {
+		orb_set_interval(_actuatorsSub, _parameters.actuator_write_period_ms);
+	}
+
 	int baudRate;
 	param_get(_param_handles.serial_baud_rate, &baudRate);
 

src/drivers/roboclaw/RoboClaw.hpp

@@ -50,6 +50,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/wheel_encoders.h>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
 #include <drivers/device/i2c.h>
 #include <sys/select.h>
 #include <sys/time.h>
@@ -129,12 +130,6 @@ class RoboClaw
 	 */
 	int resetEncoders();
 
-	/**
-	 * main update loop that updates RoboClaw motor
-	 * dutycycle based on actuator publication
-	 */
-	int update();
-
 	/**
 	 * read data from serial
 	 */
@@ -145,17 +140,8 @@ class RoboClaw
 	 */
 	void printStatus(char *string, size_t n);
 
-	void Run();
-
 private:
 
-	// Quadrature status flags
-	enum e_quadrature_status_flags {
-		STATUS_UNDERFLOW = 1 << 0, /**< encoder went below 0 **/
-		STATUS_REVERSE = 1 << 1, /**< motor doing in reverse dir **/
-		STATUS_OVERFLOW = 1 << 2, /**< encoder went above 2^32 **/
-	};
-
 	// commands
 	// We just list the commands we want from the manual here.
 	enum e_command {
@@ -182,6 +168,8 @@ class RoboClaw
 		CMD_READ_SPEED_HIRES_2 = 31,
 		CMD_SIGNED_DUTYCYCLE_1 = 32,
 		CMD_SIGNED_DUTYCYCLE_2 = 33,
+
+		CMD_READ_STATUS = 90
 	};
 
 	struct {
@@ -215,6 +203,9 @@ class RoboClaw
 	int _armedSub;
 	actuator_armed_s _actuatorArmed;
 
+	int _paramSub;
+	parameter_update_s _paramUpdate;
+
 	orb_advert_t _wheelEncodersAdv;
 	wheel_encoders_s _wheelEncoderMsg;
 

src/drivers/roboclaw/roboclaw_main.cpp

@@ -106,7 +106,7 @@ int roboclaw_main(int argc, char *argv[])
 		deamon_task = px4_task_spawn_cmd("roboclaw",
 						 SCHED_DEFAULT,
 						 SCHED_PRIORITY_MAX - 10,
-						 2500,
+						 1500,
 						 roboclaw_thread_main,
 						 (char *const *)argv);
 		return 0;
@@ -140,17 +140,12 @@ int roboclaw_thread_main(int argc, char *argv[])
 	argc -= 2;
 	argv += 2;
 
-	if (argc < 3) {
-		printf("usage: roboclaw start device address\n");
+	if (argc < 2) {
+		printf("usage: roboclaw start <device>\n");
 		return -1;
 	}
 
 	const char *deviceName = argv[1];
-	uint8_t address = strtoul(argv[2], nullptr, 0);
-	uint16_t pulsesPerRev = strtoul(argv[3], nullptr, 0);
-
-	printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n",
-	       deviceName, address, pulsesPerRev);
 
 	// start
 	RoboClaw roboclaw(deviceName);

src/drivers/roboclaw/roboclaw_params.c

@@ -75,24 +75,6 @@ PARAM_DEFINE_INT32(RBCLW_READ_PER, 10);
  */
 PARAM_DEFINE_INT32(RBCLW_COUNTS_REV, 1200);
 
-/**
- * Roboclaw serial baud rate
- *
- * Baud rate of the serial communication with the Roboclaw. The Roboclaw must be configured to match this rate.
- * @min 1
- * @max 8
- * @value 1 2400 baud
- * @value 2 9600 baud
- * @value 3 19200 baud
- * @value 4 38400 baud
- * @value 5 57600 baud
- * @value 6 115200 baud
- * @value 7 230400 baud
- * @value 8 460800 baud
- * @group Roboclaw driver
- */
-PARAM_DEFINE_INT32(RBCLW_BAUD, 8);
-
 /**
  * Communication retries
  *
@@ -132,3 +114,22 @@ PARAM_DEFINE_INT32(RBCLW_STOP_RETRY, 10);
  * @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 1
+ * @max 8
+ * @value 1 2400 baud
+ * @value 2 9600 baud
+ * @value 3 19200 baud
+ * @value 4 38400 baud
+ * @value 5 57600 baud
+ * @value 6 115200 baud
+ * @value 7 230400 baud
+ * @value 8 460800 baud
+ * @group Roboclaw driver
+ * @reboot_required true
+ */
+PARAM_DEFINE_INT32(RBCLW_BAUD, 8);