dshot: add telemetry and publish esc_status message

src/lib/mixer_module/mixer_module.cpp

@@ -374,6 +374,23 @@ MixingOutput::reorderOutputs(uint16_t values[MAX_ACTUATORS])
 	 */
 }
 
+int MixingOutput::reorderedMotorIndex(int index)
+{
+	if ((MotorOrdering)_param_mot_ordering.get() == MotorOrdering::Betaflight) {
+		switch (index) {
+		case 0: return 1;
+
+		case 1: return 2;
+
+		case 2: return 3;
+
+		case 3: return 0;
+		}
+	}
+
+	return index;
+}
+
 int MixingOutput::controlCallback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input)
 {
 	const MixingOutput *output = (const MixingOutput *)handle;
@@ -420,6 +437,8 @@ void MixingOutput::resetMixer()
 		_mixers = nullptr;
 		_groups_required = 0;
 	}
+
+	_interface.mixerChanged();
 }
 
 int MixingOutput::loadMixer(const char *buf, unsigned len)
@@ -447,6 +466,7 @@ int MixingOutput::loadMixer(const char *buf, unsigned len)
 	PX4_DEBUG("loaded mixers \n%s\n", buf);
 
 	updateParams();
+	_interface.mixerChanged();
 	return ret;
 }
 

src/lib/mixer_module/mixer_module.hpp

@@ -66,6 +66,9 @@ public:
 
 	virtual void updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
 				   unsigned num_outputs, unsigned num_control_groups_updated) = 0;
+
+	/** called whenever the mixer gets updated/reset */
+	virtual void mixerChanged() {};
 };
 
 /**
@@ -148,11 +151,18 @@ public:
 
 	uint16_t &reverseOutputMask() { return _reverse_output_mask; }
 	uint16_t &failsafeValue(int index) { return _failsafe_value[index]; }
-	/** Disarmed values: if ramp_up is true, then disarmedValue < minValue needs to hold */
+	/** Disarmed values: disarmedValue < minValue needs to hold */
 	uint16_t &disarmedValue(int index) { return _disarmed_value[index]; }
 	uint16_t &minValue(int index) { return _min_value[index]; }
 	uint16_t &maxValue(int index) { return _max_value[index]; }
 
+	/**
+	 * Get the motor index that maps from PX4 convention to the configured one
+	 * @param index motor index in [0, num_motors-1]
+	 * @return reordered motor index. When out of range, the input index is returned
+	 */
+	int reorderedMotorIndex(int index);
+
 protected:
 	void updateParams() override;