Factory method for a simple mixer that converts PWM/PPM values to the standard internal format.

2 files changed, 118 insertions, 26 deletions

diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h
index 95b576d41..26ba5db7e 100644
--- a/apps/systemlib/mixer/mixer.h
+++ b/apps/systemlib/mixer/mixer.h
@@ -243,34 +243,34 @@ public:
 	 *
 	 * Mixer definitions begin with a single capital letter and a colon.
 	 * The actual format of the mixer definition varies with the individual
-	 * mixers; they are summarised here, but see ROMFS/mixers/README for 
+	 * mixers; they are summarised here, but see ROMFS/mixers/README for
 	 * more details.
 	 *
 	 * Null Mixer
 	 * ..........
-	 * 
+	 *
 	 * The null mixer definition has the form:
-	 * 
+	 *
 	 *   Z:
-	 * 
+	 *
 	 * Simple Mixer
 	 * ............
-	 * 
+	 *
 	 * A simple mixer definition begins with:
-	 * 
+	 *
 	 *   M: <control count>
 	 *   O: <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
-	 * 
+	 *
 	 * The definition continues with <control count> entries describing the control
 	 * inputs and their scaling, in the form:
-	 * 
+	 *
 	 *   S: <group> <index> <-ve scale> <+ve scale> <offset> <lower limit> <upper limit>
-	 * 
+	 *
 	 * Multirotor Mixer
 	 * ................
-	 * 
+	 *
 	 * The multirotor mixer definition is a single line of the form:
-	 * 
+	 *
 	 * R: <geometry> <roll scale> <pitch scale> <yaw scale> <deadband>
 	 *
 	 * @param buf			The mixer configuration buffer.
@@ -335,7 +335,7 @@ public:
 	~SimpleMixer();
 
 	/**
-	 * Factory method.
+	 * Factory method with full external configuration.
 	 *
 	 * Given a pointer to a buffer containing a text description of the mixer,
 	 * returns a pointer to a new instance of the mixer.
@@ -351,9 +351,29 @@ public:
 	 *				if the text format is bad.
 	 */
 	static SimpleMixer		*from_text(Mixer::ControlCallback control_cb,
-						uintptr_t cb_handle,
-						const char *buf,
-						unsigned &buflen);
+			uintptr_t cb_handle,
+			const char *buf,
+			unsigned &buflen);
+
+	/**
+	 * Factory method for PWM/PPM input to internal float representation.
+	 *
+	 * @param control_cb		The callback to invoke when fetching a
+	 *				control value.
+	 * @param cb_handle		Handle passed to the control callback.
+	 * @param input			The control index used when fetching the input.
+	 * @param min			The PWM/PPM value considered to be "minimum" (gives -1.0 out)
+	 * @param mid			The PWM/PPM value considered to be the midpoint (gives 0.0 out)
+	 * @param max			The PWM/PPM value considered to be "maximum" (gives 1.0 out)
+	 * @return			A new SimpleMixer instance, or nullptr if one could not be
+	 *				allocated.
+	 */
+	static SimpleMixer		*pwm_input(Mixer::ControlCallback *control_cb,
+			uintptr_t cb_handle,
+			unsigned input,
+			uint16_t min,
+			uint16_t mid,
+			uint16_t max);
 
 	virtual unsigned		mix(float *outputs, unsigned space);
 	virtual void			groups_required(uint32_t &groups);
@@ -375,10 +395,10 @@ private:
 
 	static int			parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler);
 	static int			parse_control_scaler(const char *buf,
-						unsigned &buflen,
-						mixer_scaler_s &scaler,
-						uint8_t &control_group,
-						uint8_t &control_index);
+			unsigned &buflen,
+			mixer_scaler_s &scaler,
+			uint8_t &control_group,
+			uint8_t &control_index);
 };
 
 /**
@@ -457,9 +477,9 @@ public:
 	 *				if the text format is bad.
 	 */
 	static MultirotorMixer		*from_text(Mixer::ControlCallback control_cb,
-						uintptr_t cb_handle,
-						const char *buf,
-						unsigned &buflen);
+			uintptr_t cb_handle,
+			const char *buf,
+			unsigned &buflen);
 
 	virtual unsigned		mix(float *outputs, unsigned space);
 	virtual void			groups_required(uint32_t &groups);
diff --git a/apps/systemlib/mixer/mixer_simple.cpp b/apps/systemlib/mixer/mixer_simple.cpp
index c0066ac8d..05de1f6e0 100644
--- a/apps/systemlib/mixer/mixer_simple.cpp
+++ b/apps/systemlib/mixer/mixer_simple.cpp
@@ -77,9 +77,11 @@ skipspace(const char *p, unsigned &len)
 	while (isspace(*p)) {
 		if (len == 0)
 			return nullptr;
+
 		len--;
 		p++;
 	}
+
 	return p;
 }
 
@@ -91,14 +93,16 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
 	int used;
 
 	buf = skipspace(buf, buflen);
+
 	if (buflen < 16)
 		return -1;
 
 	if ((ret = sscanf(buf, "O: %d %d %d %d %d%n",
-		   &s[0], &s[1], &s[2], &s[3], &s[4], &used)) != 5) {
+			  &s[0], &s[1], &s[2], &s[3], &s[4], &used)) != 5) {
 		debug("scaler parse failed on '%s' (got %d)", buf, ret);
 		return -1;
 	}
+
 	buflen -= used;
 
 	scaler.negative_scale	= s[0] / 10000.0f;
@@ -118,6 +122,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
 	int used;
 
 	buf = skipspace(buf, buflen);
+
 	if (buflen < 16)
 		return -1;
 
@@ -126,6 +131,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
 		debug("control parse failed on '%s'", buf);
 		return -1;
 	}
+
 	buflen -= used;
 
 	control_group		= u[0];
@@ -153,13 +159,16 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
 		debug("simple parse failed on '%s'", buf);
 		goto out;
 	}
+
 	buflen -= used;
 
 	mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
+
 	if (mixinfo == nullptr) {
 		debug("could not allocate memory for mixer info");
 		goto out;
 	}
+
 	mixinfo->control_count = inputs;
 
 	if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler))
@@ -167,21 +176,84 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
 
 	for (unsigned i = 0; i < inputs; i++) {
 		if (parse_control_scaler(end - buflen, buflen,
-			mixinfo->controls[i].scaler,
-			mixinfo->controls[i].control_group,
-			mixinfo->controls[i].control_index))
+					 mixinfo->controls[i].scaler,
+					 mixinfo->controls[i].control_group,
+					 mixinfo->controls[i].control_index))
 			goto out;
 	}
 
 	sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
+
 	if (sm != nullptr) {
 		mixinfo = nullptr;
 		debug("loaded mixer with %d inputs", inputs);
+
 	} else {
 		debug("could not allocate memory for mixer");
 	}
 
 out:
+
+	if (mixinfo != nullptr)
+		free(mixinfo);
+
+	return sm;
+}
+
+SimpleMixer *
+SimpleMixer::pwm_input(Mixer::ControlCallback *control_cb, uintptr_t cb_handle, unsigned input, uint16_t min, uint16_t mid, uint16_t max)
+{
+	SimpleMixer *sm = nullptr;
+	mixer_simple_s *mixinfo = nullptr;
+
+	mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(1));
+
+	if (mixinfo == nullptr) {
+		debug("could not allocate memory for mixer info");
+		goto out;
+	}
+
+	mixinfo->control_count = 1;
+
+	/*
+	 * Always pull from group 0, with the input value giving the channel.
+	 */
+	mixinfo->controls[0].control_group = 0;
+	mixinfo->controls[0].control_index = input;
+
+	/*
+	 * Conversion uses both the input and output side of the mixer.
+	 *
+	 * The input side is used to slide the control value such that the min argument
+	 * results in a value of zero.
+	 *
+	 * The output side is used to apply the scaling for the min/max values so that
+	 * the resulting output is a -1.0 ... 1.0 value for the min...max range.
+	 */
+	mixinfo->controls[0].scaler.negative_scale = 1.0f;
+	mixinfo->controls[0].scaler.positive_scale = 1.0f;
+	mixinfo->controls[0].scaler.offset = -mid;
+	mixinfo->controls[0].scaler.lower_limit = -(mid - min);
+	mixinfo->controls[0].scaler.upper_limit = (max - mid);
+
+	mixinfo->output_scaler.negative_scale = 500.0f / (mid - min);
+	mixinfo->output_scaler.positive_scale = 500.0f / (max - mid);
+	mixinfo->output_scaler.offset = 0.0f;
+	mixinfo->output_scaler.min_output = -1.0f;
+	mixinfo->output_scaler.max_output = 1.0f;
+
+	sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
+
+	if (sm != nullptr) {
+		mixinfo = nullptr;
+		debug("PWM input mixer for %d", input);
+
+	} else {
+		debug("could not allocate memory for PWM input mixer");
+	}
+
+out:
+
 	if (mixinfo != nullptr)
 		free(mixinfo);