Roboclaw: Working temporary version that drives around

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=y
+CONFIG_COMMON_DISTANCE_SENSOR=n
 CONFIG_DRIVERS_DSHOT=y
 CONFIG_DRIVERS_ROBOCLAW=y
 CONFIG_DRIVERS_GPIO_MCP23009=y

src/drivers/roboclaw/RoboClaw.cpp

@@ -136,16 +136,28 @@ RoboClaw::~RoboClaw()
 	close(_uart);
 }
 
+void
+RoboClaw::vehicle_control_poll()
+{
+	if (_vehicle_thrust_setpoint_sub.updated()) {
+		_vehicle_thrust_setpoint_sub.copy(&vehicle_thrust_setpoint);
+	}
+	if (_vehicle_torque_setpoint_sub.updated()) {
+		_vehicle_torque_setpoint_sub.copy(&vehicle_torque_setpoint);
+	}
+}
+
+
 void RoboClaw::taskMain()
 {
 	// Make sure the Roboclaw is actually connected, so I don't just spam errors if it's not.
-	uint8_t rbuff[6];
+	// uint8_t rbuff[6];
-	int err_code = _transaction(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true);
+	// int err_code = _transaction(CMD_READ_STATUS, nullptr, 0, &rbuff[0], sizeof(rbuff), false, true);
 
-	if (err_code <= 0) {
+	// if (err_code <= 0) {
-		PX4_ERR("Unable to connect to Roboclaw. Shutting down Roboclaw driver.");
+	// 	PX4_ERR("Unable to connect to Roboclaw. Shutting down Roboclaw driver.");
-		return;
+	// 	return;
-	}
+	// }
 
 	// This main loop performs two different tasks, asynchronously:
 	// - Send actuator_controls_0 to the Roboclaw as soon as they are available
@@ -155,6 +167,9 @@ void RoboClaw::taskMain()
 	// It is updated at the end of every loop. Sometimes, if the actuator_controls_0 message came in right before
 	// I should have read the encoders, waitTime will be 0. This is fine. When waitTime is 0, poll() will return
 	// immediately with a timeout. (Or possibly with a message, if one happened to be available at that exact moment)
+
+	// printf("i am in main");
+
 	int waitTime = 100_ms;
 
 	_actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0));
@@ -173,8 +188,13 @@ void RoboClaw::taskMain()
 
 	while (!taskShouldExit) {
 
+		// printf("i am running, thrust %f \n", (double)vehicle_thrust_setpoint.xyz[0]);
+
+
 		int pret = poll(fds, sizeof(fds) / sizeof(pollfd), waitTime / 1000);
 
+		vehicle_control_poll();
+
 		if (fds[0].revents & POLLIN) {
 			orb_copy(ORB_ID(parameter_update), _paramSub, &_paramUpdate);
 			_parameters_update();
@@ -189,12 +209,14 @@ void RoboClaw::taskMain()
 					      || _actuatorArmed.force_failsafe;
 
 			if (disarmed) {
+				// printf("i am disarmed \n");
 				setMotorDutyCycle(MOTOR_1, 0.f);
 				setMotorDutyCycle(MOTOR_2, 0.f);
 
 			} else {
-				const float throttle = _actuatorControls.control[actuator_controls_s::INDEX_THROTTLE];
+				const float yaw = (double)vehicle_thrust_setpoint.xyz[0]; //temporary change
-				const float yaw = _actuatorControls.control[actuator_controls_s::INDEX_YAW];
+				// printf("thrust %f\n", (double)throttle);
+				const float throttle = (double)vehicle_torque_setpoint.xyz[2];
 				const float scale = 0.3f;
 				setMotorDutyCycle(MOTOR_1, (throttle - yaw) * scale);
 				setMotorDutyCycle(MOTOR_2, (throttle + yaw) * scale);
@@ -485,7 +507,7 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
 
 		// An error code of 0 means that select timed out, which is how the Roboclaw indicates an error.
 		if (err_code <= 0) {
-			printf("select error: %d\n", err_code);
+			// printf("select error: %d\n", err_code);
 			return err_code;
 		}
 
@@ -493,7 +515,7 @@ int RoboClaw::_transaction(e_command cmd, uint8_t *wbuff, size_t wbytes,
 		//printf("Read: %d\n", err_code);
 
 		if (err_code <= 0) {
-			printf("read error: %d\n", err_code);
+			// printf("read error: %d\n", err_code);
 			return err_code;
 
 		} else {
@@ -560,17 +582,17 @@ void RoboClaw::_parameters_update()
 		_parameters.serial_baud_rate = B2400;
 		break;
 
-	case 9600:
+	// case 9600:
-		_parameters.serial_baud_rate = B9600;
+	// 	_parameters.serial_baud_rate = B9600;
-		break;
+	// 	break;
 
-	case 19200:
+	// case 19200:
-		_parameters.serial_baud_rate = B19200;
+	// 	_parameters.serial_baud_rate = B19200;
-		break;
+	// 	break;
 
-	case 38400:
+	// case 38400:
-		_parameters.serial_baud_rate = B38400;
+	// 	_parameters.serial_baud_rate = B38400;
-		break;
+	// 	break;
 
 	case 57600:
 		_parameters.serial_baud_rate = B57600;

src/drivers/roboclaw/RoboClaw.hpp

@@ -47,15 +47,20 @@
 #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 <uORB/PublicationMulti.hpp>
 #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 <uORB/topics/vehicle_thrust_setpoint.h>
-#include <sys/select.h>
+#include <uORB/topics/vehicle_torque_setpoint.h>
-#include <sys/time.h>
+#include <uORB/Subscription.hpp>
-#include <pthread.h>
+
 
 /**
  * This is a driver for the RoboClaw motor controller
@@ -205,6 +210,14 @@ private:
 	int _paramSub{-1};
 	parameter_update_s _paramUpdate;
 
+	uORB::Subscription _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint)};
+	uORB::Subscription _vehicle_torque_setpoint_sub{ORB_ID(vehicle_torque_setpoint)};
+
+	vehicle_thrust_setpoint_s		vehicle_thrust_setpoint{};
+	vehicle_torque_setpoint_s		vehicle_torque_setpoint{};
+
+
+
 	uORB::PublicationMulti<wheel_encoders_s> _wheelEncodersAdv[2] { ORB_ID(wheel_encoders), ORB_ID(wheel_encoders)};
 	wheel_encoders_s _wheelEncoderMsg[2];
 
@@ -213,6 +226,7 @@ private:
 	int32_t _motorSpeeds[2] {0, 0};
 
 	void _parameters_update();
+	void vehicle_control_poll();
 
 	static uint16_t _calcCRC(const uint8_t *buf, size_t n, uint16_t init = 0);
 	int _sendUnsigned7Bit(e_command command, float data);

src/modules/rover_pos_control/RoverPositionControl.cpp

@@ -113,7 +113,7 @@ RoverPositionControl::vehicle_control_mode_poll()
 void
 RoverPositionControl::manual_control_setpoint_poll()
 {
-	if (_control_mode.flag_control_manual_enabled) {
+	if (true) {
 		if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) {
 			float dt = math::constrain(hrt_elapsed_time(&_manual_setpoint_last_called) * 1e-6f,  0.0002f, 0.04f);
 
@@ -431,7 +431,7 @@ RoverPositionControl::Run()
 			matrix::Vector2d current_position(_global_pos.lat, _global_pos.lon);
 			matrix::Vector3f current_velocity(_local_pos.vx, _local_pos.vy, _local_pos.vz);
 
-			if (!_control_mode.flag_control_manual_enabled && _control_mode.flag_control_position_enabled) {
+			if (false) {
 
 				if (control_position(current_position, ground_speed, _pos_sp_triplet)) {