Rovers: stop motors when commanding zero speed

PX4 · Apr 28, 2023 · d831832 · d831832
1 parent a39fb60
commit d831832
Show file tree

Hide file tree

Showing 4 changed files with 56 additions and 0 deletions.
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.cpp
@@ -45,7 +45,27 @@ ActuatorEffectivenessRoverAckermann::getEffectivenessMatrix(Configuration &confi
 	}
 
 	configuration.addActuator(ActuatorType::MOTORS, Vector3f{}, Vector3f{1.f, 0.f, 0.f});
+	_motors = 1u << 0;
 	configuration.addActuator(ActuatorType::SERVOS, Vector3f{0.f, 0.f, 1.f}, Vector3f{});
 	return true;
 }
 
+void ActuatorEffectivenessRoverAckermann::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+{
+	// Stop motors when they are commanded 0 speed
+	for (int actuator_idx = 0; actuator_idx < NUM_ACTUATORS; actuator_idx++) {
+		const uint32_t motor_mask = (1u << actuator_idx);
+
+		if (_motors & motor_mask) {
+			if (fabsf(actuator_sp(actuator_idx)) < .01f) {
+				_stopped_motors |= motor_mask;
+
+			} else {
+				_stopped_motors &= ~motor_mask;
+			}
+		}
+	}
+}
+
diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverAckermann.hpp
@@ -43,6 +43,14 @@ class ActuatorEffectivenessRoverAckermann : public ActuatorEffectiveness
 
 	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
 
+	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+
+	uint32_t getStoppedMotors() const override { return _stopped_motors; }
+
 	const char *name() const override { return "Rover (Ackermann)"; }
 private:
+	uint32_t _motors{};
+	uint32_t _stopped_motors{};
 };
diff --git a/...odules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp b/...odules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.cpp
@@ -46,6 +46,26 @@ ActuatorEffectivenessRoverDifferential::getEffectivenessMatrix(Configuration &co
 
 	configuration.addActuator(ActuatorType::MOTORS, Vector3f{0.f, 0.f, 0.5f}, Vector3f{0.5f, 0.f, 0.f});
 	configuration.addActuator(ActuatorType::MOTORS, Vector3f{0.f, 0.f, -0.5f}, Vector3f{0.5f, 0.f, 0.f});
+	_motors = (1u << 0) | (1u << 1);
 	return true;
 }
 
+void ActuatorEffectivenessRoverDifferential::updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp,
+		int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+		const matrix::Vector<float, NUM_ACTUATORS> &actuator_max)
+{
+	// Stop motors when they are commanded 0 speed
+	for (int actuator_idx = 0; actuator_idx < NUM_ACTUATORS; actuator_idx++) {
+		const uint32_t motor_mask = (1u << actuator_idx);
+
+		if (_motors & motor_mask) {
+			if (fabsf(actuator_sp(actuator_idx)) < .01f) {
+				_stopped_motors |= motor_mask;
+
+			} else {
+				_stopped_motors &= ~motor_mask;
+			}
+		}
+	}
+}
+
diff --git a/...odules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp b/...odules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRoverDifferential.hpp
@@ -43,6 +43,14 @@ class ActuatorEffectivenessRoverDifferential: public ActuatorEffectiveness
 
 	bool getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) override;
 
+	void updateSetpoint(const matrix::Vector<float, NUM_AXES> &control_sp, int matrix_index,
+			    ActuatorVector &actuator_sp, const matrix::Vector<float, NUM_ACTUATORS> &actuator_min,
+			    const matrix::Vector<float, NUM_ACTUATORS> &actuator_max) override;
+
+	uint32_t getStoppedMotors() const override { return _stopped_motors; }
+
 	const char *name() const override { return "Rover (Differential)"; }
 private:
+	uint32_t _motors{};
+	uint32_t _stopped_motors{};
 };