Work in progress; standard mixer API and utility

apps/systemlib/mixer.c

@@ -43,22 +43,31 @@
  * See mixer.h for more details.
  */
 
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+
 #include "mixer.h"
 
 static int
 scale_check(struct MixScaler *scale)
 {
- 	if (scale->offset > 1.0f)
- 		return -1;
- 	if (scale->offset > 1.0f)
- 		return -1;
- 	if (scale->lower_limit > scale->upper_limit)
- 		return -1;
- 	if (scale->lower_limit < -1.0f)
- 		return -1;
- 	if (scale->upper_limit > 1.0f)
- 		return -1;
- 	return 0;
+	if (scale->offset > 1.0f)
+		return -1;
+
+	if (scale->offset > 1.0f)
+		return -1;
+
+	if (scale->lower_limit > scale->upper_limit)
+		return -1;
+
+	if (scale->lower_limit < -1.0f)
+		return -1;
+
+	if (scale->upper_limit > 1.0f)
+		return -1;
+
+	return 0;
 }
 
 int
@@ -66,17 +75,21 @@ mixer_check(struct MixMixer *mixer, unsigned control_count)
 {
 	if (mixer->control_count < 1)
 		return -1;
+
 	if (mixer->control_count > control_count)
 		return -1;
+
 	if (!scale_check(&mixer->output_scaler))
 		return -1;
 
 	for (unsigned i = 0; i < mixer->control_count; i++) {
 		if (mixer->control_scaler[i].control >= control_count)
 			return -1;
+
 		if (!scale_check(&mixer->control_scaler[i]))
 			return -1;
 	}
+
 	return 0;
 }
 
@@ -87,11 +100,14 @@ scale(struct MixScaler *scaler, float input)
 
 	if (input < 0.0f) {
 		output = (input * scaler->negative_scale) + scaler->offset;
+
 	} else {
 		output = (input * scaler->positive_scale) + scaler->offset;
 	}
+
 	if (output > scaler->upper_limit) {
 		output = scaler->upper_limit;
+
 	} else if (output < scaler->lower_limit) {
 		output = scaler->lower_limit;
 	}
@@ -112,3 +128,132 @@ mixer_mix(struct MixMixer *mixer, float *controls)
 
 	return scale(&mixer->output_scaler, sum);
 }
+
+static int
+mixer_getline(int fd, char *line, unsigned maxlen)
+{
+	int	ret;
+	char	c;
+
+	while (--maxlen) {
+		ret = read(fd, &c, 1);
+		if (ret <= 0)
+			return ret;
+		if (c == '\r')
+			continue;
+		if (c == '\n') {
+			*line = '\0';
+			return 1;
+		}
+		*line++ = c;
+	}
+	/* line too long */
+	return -1;
+}
+
+static int
+mixer_load_scaler(const char *buf, struct MixScaler *scaler)
+{
+	if (sscanf(buf, "S: %u %f %f %f %f %f",
+		   &scaler->control, &scaler->negative_scale, &scaler->positive_scale,
+		   &scaler->offset, &scaler->lower_limit, &scaler->upper_limit) != 6)
+		return -1;
+
+	return 0;
+}
+
+int
+mixer_load(int fd, struct MixMixer **mp)
+{
+	int		ret, result = -1;
+	struct MixMixer *mixer = NULL;
+	char		buf[100];
+	unsigned	scalers;
+
+	ret = mixer_getline(fd, buf, sizeof(buf));
+
+	/* end of file? */
+	if (ret == 0)
+		result = 0;
+
+	/* can't proceed */
+	if (ret < 1)
+		goto out;
+
+	/* get header */
+	if (sscanf(buf, "M: %u", &scalers) != 1)
+		goto out;
+
+	/* must have at least one scaler */
+	if (scalers < 1)
+		goto out;
+
+	/* allocate mixer */
+	scalers--;
+	mixer = (struct MixMixer *)malloc(MIXER_SIZE(scalers));
+
+	if (mixer == NULL)
+		goto out;
+
+	mixer->control_count = scalers;
+
+	ret = mixer_getline(fd, buf, sizeof(buf));
+
+	if (ret < 1)
+		goto out;
+
+	if (mixer_load_scaler(buf, &mixer->output_scaler))
+		goto out;
+
+	for (unsigned i = 0; i < scalers; i++) {
+		if (mixer_getline(fd, buf, sizeof(buf)))
+			goto out;
+		if (mixer_load_scaler(buf, &mixer->control_scaler[i]))
+			goto out;
+	}
+
+	result = 1;
+
+out:
+	/* on error, discard allocated mixer */
+	if ((result <= 0) && (mixer != NULL))
+		free(mixer);
+	*mp = mixer;
+	return result;
+}
+
+static int
+mixer_save_scaler(char *buf, struct MixScaler *scaler)
+{
+	return sprintf(buf, "S: %u %f %f %f %f %f\n",
+		       scaler->control, scaler->negative_scale, scaler->positive_scale,
+		       scaler->offset, scaler->lower_limit, scaler->upper_limit);
+}
+
+int
+mixer_save(int fd, struct MixMixer *mixer)
+{
+	char		buf[100];
+	int		len, ret;
+	
+	/* write the mixer header */
+	len = sprintf(buf, "M: %u\n", mixer->control_count);
+	ret = write(fd, buf, len);
+	if (ret != len)
+		return -1;
+
+	/* write the output scaler */
+	len = mixer_save_scaler(buf, &mixer->output_scaler);
+	write(fd, buf, len);
+	if (ret != len)
+		return -1;
+
+	/* write the control scalers */
+	for (unsigned j = 0; j < mixer->control_count; j++) {
+		len = mixer_save_scaler(buf, &mixer->control_scaler[j]);
+		write(fd, buf, len);
+		if (ret != len)
+			return -1;
+	}
+	return 0;
+}

apps/systemlib/mixer.h

@@ -31,6 +31,9 @@
  *
  ****************************************************************************/
 
+#ifndef _SYSTEMLIB_MIXER_H
+#define _SYSTEMLIB_MIXER_H
+
 /**
  * @file mixer.h
  * 
@@ -55,7 +58,7 @@
  *
  * An actuator derives its value from the combination of one or more
  * control values. Each of the control values is scaled according to
- * the actuator's configuration and then combined to produce the 
+ * the actuator's configuration and then combined to produce the
  * actuator value, which may then be further scaled to suit the specific
  * output type.
  *
@@ -100,7 +103,7 @@
  * Mixing
  * ------
  *
- * Mixing is performed by summing the scaled control values.  
+ * Mixing is performed by summing the scaled control values.
  *
  *
  * Controls
@@ -109,7 +112,7 @@
  * Each mixer is presented with an array of controls from which it
  * selects the set that will be mixed for each actuator.
  *
- * The precise assignment of controls may vary depending on the 
+ * The precise assignment of controls may vary depending on the
  * application, but the following assignments should be used
  * when appropriate.
  *
@@ -121,22 +124,25 @@
  *     3   | primary thrust
  */
 
- struct MixScaler
- {
- 	unsigned	control;	/**< control consumed by this scaler */
- 	float		negative_scale;	/**< scale for inputs < 0 */
- 	float		positive_scale;	/**< scale for inputs > 0 */
- 	float		offset;		/**< bias applied to output */
- 	float		lower_limit;	/**< minimum output value */
- 	float		upper_limit;	/**< maximum output value */
- };
+struct MixScaler {
+	unsigned	control;	/**< control consumed by this scaler */
+	float		negative_scale;	/**< scale for inputs < 0 */
+	float		positive_scale;	/**< scale for inputs > 0 */
+	float		offset;		/**< bias applied to output */
+	float		lower_limit;	/**< minimum output value */
+	float		upper_limit;	/**< maximum output value */
+};
 
- struct MixMixer
- {
- 	unsigned		control_count;	/**< number of control scalers */
- 	struct MixScaler	output_scaler;	/**< scaler applied to mixer output */
- 	struct MixScaler	control_scaler[0]; /**< array of control scalers */
- };
+struct MixMixer {
+	unsigned		control_count;	/**< number of control scalers */
+	struct MixScaler	output_scaler;	/**< scaler applied to mixer output */
+	struct MixScaler	control_scaler[0]; /**< array of control scalers */
+};
+
+/**
+ * Handy macro for determining the allocation size of a mixer.
+ */
+#define MIXER_SIZE(_num_scalers)	(sizeof(struct MixMixer) + ((_num_scalers) * sizeof(struct MixScaler)))
 
 __BEGIN_DECLS
 
@@ -150,16 +156,55 @@ __BEGIN_DECLS
  * @param controls		Array of input control values.
  * @return			The mixed output.
  */
- __EXPORT float	mixer_mix(struct MixMixer *mixer, float *controls);
+__EXPORT float	mixer_mix(struct MixMixer *mixer, float *controls);
+
+/**
+ * Check a mixer configuration for sanity.
+ *
+ * @param mixer		The mixer configuration to be checked.
+ * @param control_count	The number of controls in the system.
+ * @return			Zero if the mixer configuration is sane,
+ *				nonzero otherwise.
+ */
+__EXPORT int	mixer_check(struct MixMixer *mixer, unsigned control_count);
+
+/**
+ * Read a mixer definition from a file.
+ *
+ * A mixer definition is a text representation of the configuration of a
+ * mixer.  The definition consists of a single-line header indicating the
+ * number of scalers and then one line defining each scaler.  The first
+ * scaler in the file is always the output scaler, followed by the input
+ * scalers.
+ *
+ * M: <control count + 1>
+ * S: <control> <negative_scale> <positive_scale> <offset> <lower_limit> <upper_limit>
+ * S: ...
+ *
+ * The <control> value for the output scaler is ignored.
+ *
+ * Multiple mixer definitions may be stored in a single file; it is assumed that
+ * the reader will know how many to expect and read accordingly. Mixers may be
+ * 'skipped' in a file by  setting indicating that the mixer has only one scaler 
+ * (the output scaler). This results in a mixer with zero controls, which will
+ * always generate output corresponding to the output scaler offset.
+ *
+ * @param fd			The file to read the definitions from.
+ * @param mixer			Mixer is returned here.
+ * @return			1 if a mixer was read, zero on EOF or negative on error.
+ */
+__EXPORT int	mixer_load(int fd, struct MixMixer **mixer);
+
+/**
+ * Save a mixer definition to a file.
+ *
+ * @param fd			The file to write the definitions to.
+ * @param mixer			The mixer definition to save.
+ * @return			Zero on success, negative on error.
+ */
+__EXPORT int	mixer_save(int fd, struct MixMixer *mixers);
 
- /**
-  * Check a mixer configuration for sanity.
-  *
-  * @param mixer		The mixer configuration to be checked.
-  * @param control_count	The number of controls in the system.
-  * @return			Zero if the mixer configuration is sane,
-  *				nonzero otherwise.
-  */
- __EXPORT int	mixer_check(struct MixMixer *mixer, unsigned control_count);
 
 __END_DECLS
+
+#endif /* _SYSTEMLIB_MIXER_H */