From c3464bb9afc5e087a1732e7fdc54709a48e780a1 Mon Sep 17 00:00:00 2001
From: haitomatic <hai.to@unikie.com>
Date: Wed, 4 Oct 2023 07:02:20 +0000
Subject: [PATCH] rover interface:use vehicle thrust and torque setpoint
 instead of actuator controls

---
 .../rover_interface/RoverInterface.cpp        | 42 +++++++++++++------
 .../rover_interface/RoverInterface.hpp        | 16 ++++---
 2 files changed, 41 insertions(+), 17 deletions(-)

diff --git a/src/drivers/rover_interface/RoverInterface.cpp b/src/drivers/rover_interface/RoverInterface.cpp
index 171e9b5c1307..8d65ea989abd 100644
--- a/src/drivers/rover_interface/RoverInterface.cpp
+++ b/src/drivers/rover_interface/RoverInterface.cpp
@@ -200,7 +200,7 @@ void RoverInterface::Run()
 	ActionRequestUpdate();
 
 	// Check for actuator controls command to rover
-	if (!_kill_switch && _armed) { ActuatorControlsUpdate(); }
+	if (!_kill_switch && _armed) { VehicleTorqueAndThrustUpdate(); }
 
 	// Check for vehicle control mode
 	VehicleControlModeUpdate();
@@ -219,18 +219,35 @@ void RoverInterface::Run()
 }
 
 
-void RoverInterface::ActuatorControlsUpdate()
+void RoverInterface::VehicleTorqueAndThrustUpdate()
 {
-	if (_actuator_controls_sub.updated()) {
-		actuator_controls_s actuator_controls_msg;
-
-		if (_actuator_controls_sub.copy(&actuator_controls_msg)) {
-			auto throttle = (_is_manual_mode ? _manual_throttle_max : _mission_throttle_max) *
-					actuator_controls_msg.control[actuator_controls_s::INDEX_THROTTLE];
-			auto steering = actuator_controls_msg.control[actuator_controls_s::INDEX_YAW];
-			_scout->SetMotionCommand(throttle, steering);
+	bool do_update = false;
+
+	// Check torque setppoint update
+	if (_vehicle_torque_setpoint_sub.updated()) {
+		vehicle_torque_setpoint_s vehicle_torque_setpoint_msg;
+
+		if (_vehicle_torque_setpoint_sub.copy(&vehicle_torque_setpoint_msg)) {
+			_yaw_control = vehicle_torque_setpoint_msg.xyz[2];
+			do_update = true;
+		}
+	}
+
+	// Check thrust setpoint update
+	if (_vehicle_thrust_setpoint_sub.updated()) {
+		vehicle_thrust_setpoint_s vehicle_thrust_setpoint_msg;
+
+		if (_vehicle_thrust_setpoint_sub.copy(&vehicle_thrust_setpoint_msg)) {
+			_throttle_control = vehicle_thrust_setpoint_msg.xyz[0];
+			do_update = true;
 		}
 	}
+
+	if (do_update) {
+		auto throttle = (_is_manual_mode ? _manual_throttle_max : _mission_throttle_max) * _throttle_control;
+		auto steering = _yaw_control;
+		_scout->SetMotionCommand(throttle, steering);
+	}
 }
 
 
@@ -340,8 +357,9 @@ void RoverInterface::print_status()
 	PX4_INFO("Rover is armed: %s. Kill switch: %s", _armed ? "true" : "false", _kill_switch ? "true" : "false");
 
 	// Subscription info
-	PX4_INFO("Subscribed to topics: %s, %s",
-		 _actuator_controls_sub.get_topic()->o_name,
+	PX4_INFO("Subscribed to topics: %s, %s, %s",
+		 _vehicle_thrust_setpoint_sub.get_topic()->o_name,
+		 _vehicle_torque_setpoint_sub.get_topic()->o_name,
 		 _actuator_armed_sub.get_topic()->o_name);
 
 	// Publication info
diff --git a/src/drivers/rover_interface/RoverInterface.hpp b/src/drivers/rover_interface/RoverInterface.hpp
index 3fa28989627e..c55a6d4695f1 100644
--- a/src/drivers/rover_interface/RoverInterface.hpp
+++ b/src/drivers/rover_interface/RoverInterface.hpp
@@ -11,7 +11,8 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/Publication.hpp>
-#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_thrust_setpoint.h>
+#include <uORB/topics/vehicle_torque_setpoint.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/action_request.h>
 #include <uORB/topics/rover_status.h>
@@ -33,7 +34,7 @@ class RoverInterface : public ModuleParams, public px4::ScheduledWorkItem
 
 	static constexpr uint64_t SystemVersionQueryLimitMs{10_ms};
 
-	static constexpr uint64_t ActuatorControlSubIntervalMs{50_ms};
+	static constexpr uint64_t ControlSubIntervalMs{50_ms};
 
 public:
 	static const char *const CAN_IFACE;
@@ -51,7 +52,7 @@ class RoverInterface : public ModuleParams, public px4::ScheduledWorkItem
 	void Init();
 	void Run() override;
 
-	void ActuatorControlsUpdate();
+	void VehicleTorqueAndThrustUpdate();
 	void ActuatorArmedUpdate();
 	void ActionRequestUpdate();
 	void VehicleControlModeUpdate();
@@ -82,6 +83,10 @@ class RoverInterface : public ModuleParams, public px4::ScheduledWorkItem
 
 	float _mission_throttle_max;
 
+	float _throttle_control;
+
+	float _yaw_control;
+
 	scoutsdk::ProtocolVersion _protocol_version{scoutsdk::ProtocolVersion::AGX_V2};
 
 	const char *_can_iface{nullptr};
@@ -89,8 +94,9 @@ class RoverInterface : public ModuleParams, public px4::ScheduledWorkItem
 	scoutsdk::ScoutRobot *_scout{nullptr};
 
 	// Subscription
-	uORB::SubscriptionInterval _actuator_controls_sub{ORB_ID(actuator_controls_0), ActuatorControlSubIntervalMs};
-	uORB::SubscriptionInterval _actuator_armed_sub{ORB_ID(actuator_armed)};
+	uORB::SubscriptionInterval _vehicle_thrust_setpoint_sub{ORB_ID(vehicle_thrust_setpoint), ControlSubIntervalMs};
+	uORB::SubscriptionInterval _vehicle_torque_setpoint_sub{ORB_ID(vehicle_torque_setpoint), ControlSubIntervalMs};
+	uORB::Subscription _actuator_armed_sub{ORB_ID(actuator_armed)};
 	uORB::Subscription _action_request_sub{ORB_ID(action_request)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};