only enable offboard for position and velocity setpoints

PX4 · Feb 4, 2025 · 0910c52 · 0910c52
1 parent 7037a5f
commit 0910c52
Show file tree

Hide file tree

Showing 4 changed files with 6 additions and 53 deletions.
diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.cpp
@@ -84,7 +84,7 @@ void AckermannAttControl::updateAttControl()
 						   (actuator_motors.control[0], -1.f, 1.f, -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()) : 0.f;
 		}
 
-		if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) {
+		if (_vehicle_control_mode.flag_control_manual_enabled) {
 			generateAttitudeSetpoint();
 		}
 
@@ -150,23 +150,6 @@ void AckermannAttControl::generateAttitudeSetpoint()
 
 		}
 
-	} else if (
-		_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Rate Control TODO: This doesn't work without also setting speed or throttle
-		if (_offboard_control_mode_sub.updated()) {
-			_offboard_control_mode_sub.copy(&_offboard_control_mode);
-		}
-
-		bool offboard_attitude_control = _offboard_control_mode.position && !_offboard_control_mode.velocity
-						 && _offboard_control_mode.attitude;
-
-		if (offboard_attitude_control && _trajectory_setpoint_sub.updated()) {
-			trajectory_setpoint_s trajectory_setpoint{};
-			_trajectory_setpoint_sub.copy(&trajectory_setpoint);
-			rover_attitude_setpoint_s rover_attitude_setpoint{};
-			rover_attitude_setpoint.timestamp = _timestamp;
-			rover_attitude_setpoint.yaw_setpoint = trajectory_setpoint.yaw;
-			_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
-		}
 	}
 }
 

diff --git a/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp b/src/modules/rover_ackermann/AckermannAttControl/AckermannAttControl.hpp
@@ -48,9 +48,7 @@
 #include <uORB/topics/rover_throttle_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/rover_attitude_status.h>
 #include <uORB/topics/rover_attitude_setpoint.h>
 #include <uORB/topics/actuator_motors.h>
@@ -81,8 +79,7 @@ class AckermannAttControl : public ModuleParams
 
 private:
 	/**
-	 * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Stab Mode)
-	 *        or trajectorySetpoint (Offboard attitude control).
+	 * @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Stab Mode).
 	 */
 	void generateAttitudeSetpoint();
 
@@ -94,14 +91,11 @@ class AckermannAttControl : public ModuleParams
 	// uORB subscriptions
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
-	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
-	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
 	uORB::Subscription _rover_attitude_setpoint_sub{ORB_ID(rover_attitude_setpoint)};
 	uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)};
 	vehicle_control_mode_s _vehicle_control_mode{};
-	offboard_control_mode_s _offboard_control_mode{};
 	rover_attitude_setpoint_s _rover_attitude_setpoint{};
 	rover_rate_setpoint_s _rover_rate_setpoint{};
 

diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.cpp
@@ -72,17 +72,16 @@ void AckermannRateControl::updateRateControl()
 	}
 
 	if (_vehicle_control_mode.flag_control_rates_enabled  && _vehicle_control_mode.flag_armed) {
-		// Estimate forward speed based on throttle TODO: Use speed measurement if available ?
+		// Estimate forward speed based on throttle
 		if (_actuator_motors_sub.updated()) {
 			actuator_motors_s actuator_motors;
 			_actuator_motors_sub.copy(&actuator_motors);
 			_estimated_forward_speed = _param_ro_max_thr_speed.get() > FLT_EPSILON ? math::interpolate<float>
 						   (actuator_motors.control[0], -1.f, 1.f, -_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get()) : 0.f;
-			_estimated_forward_speed = fabsf(_estimated_forward_speed) >  _param_ro_speed_th.get() ? _estimated_forward_speed :
-						   0.f; // TODO: This is supposed to help with not perfectly calibrated sticks, is there a better way?
+			_estimated_forward_speed = fabsf(_estimated_forward_speed) >  _param_ro_speed_th.get() ? _estimated_forward_speed : 0.f;
 		}
 
-		if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) {
+		if (_vehicle_control_mode.flag_control_manual_enabled) {
 			generateRateSetpoint();
 		}
 
@@ -127,23 +126,6 @@ void AckermannRateControl::generateRateSetpoint()
 			_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
 		}
 
-	} else if (
-		_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard Rate Control TODO: This doesn't work without also setting speed or throttle
-		if (_offboard_control_mode_sub.updated()) {
-			_offboard_control_mode_sub.copy(&_offboard_control_mode);
-		}
-
-		bool offboard_rate_control = _offboard_control_mode.body_rate && !_offboard_control_mode.position
-					     && !_offboard_control_mode.velocity && !_offboard_control_mode.attitude;
-
-		if (offboard_rate_control && _trajectory_setpoint_sub.updated()) {
-			trajectory_setpoint_s trajectory_setpoint{};
-			_trajectory_setpoint_sub.copy(&trajectory_setpoint);
-			rover_rate_setpoint_s rover_rate_setpoint{};
-			rover_rate_setpoint.timestamp = _timestamp;
-			rover_rate_setpoint.yaw_rate_setpoint = trajectory_setpoint.yawspeed;
-			_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
-		}
 	}
 }
 

diff --git a/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp b/src/modules/rover_ackermann/AckermannRateControl/AckermannRateControl.hpp
@@ -49,9 +49,7 @@
 #include <uORB/topics/rover_throttle_setpoint.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
-#include <uORB/topics/offboard_control_mode.h>
 #include <uORB/topics/rover_steering_setpoint.h>
 #include <uORB/topics/rover_rate_status.h>
 #include <uORB/topics/actuator_motors.h>
@@ -83,8 +81,7 @@ class AckermannRateControl : public ModuleParams
 private:
 
 	/**
-	 * @brief Generate and publish roverRateSetpoint from manualControlSetpoint (Acro Mode)
-	 *        or trajectorySetpoint (Offboard rate control).
+	 * @brief Generate and publish roverRateSetpoint from manualControlSetpoint (Acro Mode).
 	 */
 	void generateRateSetpoint();
 
@@ -97,12 +94,9 @@ class AckermannRateControl : public ModuleParams
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _rover_rate_setpoint_sub{ORB_ID(rover_rate_setpoint)};
-	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
-	uORB::Subscription _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
 	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
 	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
 	vehicle_control_mode_s _vehicle_control_mode{};
-	offboard_control_mode_s _offboard_control_mode{};
 	rover_rate_setpoint_s _rover_rate_setpoint{};
 
 	// uORB publications