actuator_motors.msg: add reversible flags & implement in mixer_module

msg/actuator_motors.msg

@@ -2,6 +2,8 @@
 uint64 timestamp			# time since system start (microseconds)
 uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
 
+uint16 reversible_flags     # bitset which motors are configured to be reversible
+
 uint8 NUM_CONTROLS = 8
 float32[8] control # range: [-1, 1], where 1 means maximum positive thrust,
                    # -1 maximum negative (if not supported by the output, <0 maps to NaN),

src/drivers/dshot/DShot.cpp

@@ -444,6 +444,8 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 	} else {
 		int telemetry_index = 0;
 
+		const uint32_t reversible_outputs = _mixing_output.reversibleOutputs();
+
 		for (int i = 0; i < (int)num_outputs; i++) {
 
 			uint16_t output = outputs[i];
@@ -452,7 +454,7 @@ bool DShot::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 			// This is in terms of DShot values, code below is in terms of actuator_output
 			// Direction 1) 48 is the slowest, 1047 is the fastest.
 			// Direction 2) 1049 is the slowest, 2047 is the fastest.
-			if (_param_dshot_3d_enable.get()) {
+			if (_param_dshot_3d_enable.get() || (reversible_outputs & (1u << i))) {
 				if (output >= _param_dshot_3d_dead_l.get() && output <= _param_dshot_3d_dead_h.get()) {
 					output = DSHOT_DISARM_VALUE;
 

src/lib/mixer_module/actuator_test.cpp

@@ -41,7 +41,7 @@ ActuatorTest::ActuatorTest(const OutputFunction function_assignments[MAX_ACTUATO
 	reset();
 }
 
-void ActuatorTest::update(int num_outputs, bool reversible_motors, float thrust_curve)
+void ActuatorTest::update(int num_outputs, float thrust_curve)
 {
 	const hrt_abstime now = hrt_absolute_time();
 
@@ -74,7 +74,11 @@ void ActuatorTest::update(int num_outputs, bool reversible_motors, float thrust_
 
 					// handle motors
 					if (actuator_test.function >= (int)OutputFunction::Motor1 && actuator_test.function <= (int)OutputFunction::MotorMax) {
-						FunctionMotors::updateValues(reversible_motors, thrust_curve, &value, 1);
+						actuator_motors_s motors;
+						motors.reversible_flags = 0;
+						_actuator_motors_sub.copy(&motors);
+						int motor_idx = actuator_test.function - (int)OutputFunction::Motor1;
+						FunctionMotors::updateValues(motors.reversible_flags >> motor_idx, thrust_curve, &value, 1);
 					}
 
 					_current_outputs[i] = value;

src/lib/mixer_module/actuator_test.hpp

@@ -37,6 +37,7 @@
 
 #include <drivers/drv_pwm_output.h>
 #include <uORB/topics/actuator_test.h>
+#include <uORB/topics/actuator_motors.h>
 #include <uORB/Subscription.hpp>
 
 static_assert(actuator_test_s::FUNCTION_MOTOR1 == (int)OutputFunction::Motor1, "define mismatch");
@@ -55,7 +56,7 @@ public:
 
 	void reset();
 
-	void update(int num_outputs, bool reversible_motors, float thrust_curve);
+	void update(int num_outputs, float thrust_curve);
 
 	void overrideValues(float outputs[MAX_ACTUATORS], int num_outputs);
 
@@ -64,6 +65,7 @@ public:
 private:
 
 	uORB::Subscription _actuator_test_sub{ORB_ID(actuator_test)};
+	uORB::Subscription _actuator_motors_sub{ORB_ID(actuator_motors)};
 	bool _in_test_mode{false};
 	hrt_abstime _next_timeout{0};
 

src/lib/mixer_module/functions.cpp

@@ -35,7 +35,6 @@
 
 FunctionMotors::FunctionMotors(const Context &context)
 	: _topic(&context.work_item, ORB_ID(actuator_motors)),
-	  _reversible_motors(context.reversible_motors),
 	  _thrust_factor(context.thrust_factor)
 {
 	for (int i = 0; i < actuator_motors_s::NUM_CONTROLS; ++i) {
@@ -45,7 +44,7 @@ FunctionMotors::FunctionMotors(const Context &context)
 void FunctionMotors::update()
 {
 	if (_topic.update(&_data)) {
-		updateValues(_reversible_motors, _thrust_factor, _data.control, actuator_motors_s::NUM_CONTROLS);
+		updateValues(_data.reversible_flags, _thrust_factor, _data.control, actuator_motors_s::NUM_CONTROLS);
 	}
 }
 

src/lib/mixer_module/functions.hpp

@@ -52,7 +52,6 @@ class FunctionProviderBase
 public:
 	struct Context {
 		px4::WorkItem &work_item;
-		bool reversible_motors;
 		const float &thrust_factor;
 	};
 
@@ -73,6 +72,11 @@ public:
 	virtual uORB::SubscriptionCallbackWorkItem *subscriptionCallback() { return nullptr; }
 
 	virtual bool getLatestSampleTimestamp(hrt_abstime &t) const { return false; }
+
+	/**
+	 * Check whether the output (motor) is configured to be reversible
+	 */
+	virtual bool reversible(OutputFunction func) const { return false; }
 };
 
 /**
@@ -124,15 +128,17 @@ public:
 
 	bool getLatestSampleTimestamp(hrt_abstime &t) const override { t = _data.timestamp_sample; return t != 0; }
 
-	static inline void updateValues(bool reversible, float thrust_factor, float *values, int num_values);
+	static inline void updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values);
+
+	bool reversible(OutputFunction func) const override
+	{ return _data.reversible_flags & (1u << ((int)func - (int)OutputFunction::Motor1)); }
 private:
 	uORB::SubscriptionCallbackWorkItem _topic;
 	actuator_motors_s _data{};
-	const bool _reversible_motors;
 	const float &_thrust_factor;
 };
 
-void FunctionMotors::updateValues(bool reversible, float thrust_factor, float *values, int num_values)
+void FunctionMotors::updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values)
 {
 	if (thrust_factor > FLT_EPSILON) {
 		for (int i = 0; i < num_values; ++i) {
@@ -143,8 +149,8 @@ void FunctionMotors::updateValues(bool reversible, float thrust_factor, float *v
 		}
 	}
 
-	if (!reversible) {
-		for (int i = 0; i < num_values; ++i) {
+	for (int i = 0; i < num_values; ++i) {
+		if ((reversible & (1u << i)) == 0) {
 			if (values[i] < -FLT_EPSILON) {
 				values[i] = NAN;
 

src/lib/mixer_module/mixer_module.cpp

@@ -385,7 +385,7 @@ bool MixingOutput::updateSubscriptionsDynamicMixer(bool allow_wq_switch, bool li
 
 	cleanupFunctions();
 
-	const FunctionProviderBase::Context context{_interface, _reversible_motors, _param_thr_mdl_fac.reference()};
+	const FunctionProviderBase::Context context{_interface, _param_thr_mdl_fac.reference()};
 	int provider_indexes[MAX_ACTUATORS] {};
 	int next_provider = 0;
 	int subscription_callback_provider_index = INT_MAX;
@@ -776,11 +776,12 @@ bool MixingOutput::updateDynamicMixer()
 	}
 
 	// check for actuator test
-	_actuator_test.update(_max_num_outputs, _reversible_motors, _param_thr_mdl_fac.get());
+	_actuator_test.update(_max_num_outputs, _param_thr_mdl_fac.get());
 
 	// get output values
 	float outputs[MAX_ACTUATORS];
 	bool all_disabled = true;
+	_reversible_mask = 0;
 
 	for (int i = 0; i < _max_num_outputs; ++i) {
 		if (_functions[i]) {
@@ -793,6 +794,8 @@ bool MixingOutput::updateDynamicMixer()
 				outputs[i] = NAN;
 			}
 
+			_reversible_mask |= (uint32_t)_functions[i]->reversible(_function_assignment[i]) << i;
+
 		} else {
 			outputs[i] = NAN;
 		}

src/lib/mixer_module/mixer_module.hpp

@@ -82,7 +82,7 @@ public:
 				   unsigned num_outputs, unsigned num_control_groups_updated) = 0;
 
 	/** called whenever the mixer gets updated/reset */
-	virtual void mixerChanged() {};
+	virtual void mixerChanged() {}
 };
 
 /**
@@ -215,6 +215,12 @@ public:
 
 	void setLowrateSchedulingInterval(hrt_abstime interval) { _lowrate_schedule_interval = interval; }
 
+	/**
+	 * Get the bitmask of reversible outputs (motors only).
+	 * This might change at any time (while disarmed), so output drivers requiring this should query this regularly.
+	 */
+	uint32_t reversibleOutputs() const { return _reversible_mask; }
+
 protected:
 	void updateParams() override;
 
@@ -338,12 +344,11 @@ private:
 	bool _need_function_update{true};
 	bool _use_dynamic_mixing{false}; ///< set to _param_sys_ctrl_alloc on init (avoid changing after startup)
 	bool _has_backup_schedule{false};
-	bool _reversible_motors =
-		false; ///< whether or not the output module supports reversible motors (range [-1, 0] for motors)
 	const char *const _param_prefix;
 	ParamHandles _param_handles[MAX_ACTUATORS];
 	hrt_abstime _lowrate_schedule_interval{300_ms};
 	ActuatorTest _actuator_test{_function_assignment};
+	uint32_t _reversible_mask{0}; ///< per-output bits. If set, the output is configured to be reversible (motors only)
 
 	uORB::SubscriptionCallbackWorkItem *_subscription_callback{nullptr}; ///< current scheduling callback