Merge branch 'master' into inav_sonar_indep

ROMFS/px4fmu_common/init.d/1000_rc_fw.hil

@@ -33,6 +33,8 @@ then
 	param set FW_T_SINK_MIN 4.0
 	param set FW_Y_ROLLFF 1.1
 	param set FW_L1_PERIOD 16
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
 
 	param set SYS_AUTOCONFIG 0
 	param save

ROMFS/px4fmu_common/init.d/100_mpx_easystar

@@ -29,6 +29,8 @@ then
 	param set FW_T_SINK_MIN 4.0
 	param set FW_Y_ROLLFF 1.1
 	param set FW_L1_PERIOD 16
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
 	
 	param set SYS_AUTOCONFIG 0
 	param save
@@ -98,8 +100,7 @@ else
 	mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix
 fi
 fw_att_control start
-# Not ready yet for prime-time
+fw_pos_control_l1 start
-#fw_pos_control_l1 start
 
 if [ $EXIT_ON_END == yes ]
 then

ROMFS/px4fmu_common/init.d/101_hk_bixler

@@ -29,6 +29,8 @@ then
 	param set FW_T_SINK_MIN 4.0
 	param set FW_Y_ROLLFF 1.1
 	param set FW_L1_PERIOD 16
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
 	
 	param set SYS_AUTOCONFIG 0
 	param save
@@ -91,8 +93,7 @@ att_pos_estimator_ekf start
 #
 mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
 fw_att_control start
-# Not ready yet for prime-time
+fw_pos_control_l1 start
-#fw_pos_control_l1 start
 
 if [ $EXIT_ON_END == yes ]
 then

ROMFS/px4fmu_common/init.d/31_io_phantom

@@ -29,6 +29,8 @@ then
 	param set FW_T_SINK_MIN 4.0
 	param set FW_Y_ROLLFF 1.1
 	param set FW_L1_PERIOD 17
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
 	
 	param set SYS_AUTOCONFIG 0
 	param save

Tools/tests-host/.gitignore

@@ -0,0 +1,2 @@
+./obj/*
+mixer_test

Tools/tests-host/Makefile

@@ -0,0 +1,39 @@
+
+CC=g++
+CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers -I../../src -D__EXPORT="" -Dnullptr="0"
+
+ODIR=obj
+LDIR =../lib
+
+LIBS=-lm
+
+#_DEPS = test.h
+#DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS))
+
+_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o mixer.o mixer_group.o mixer_load.o
+OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ))
+
+#$(DEPS)
+$(ODIR)/%.o: %.cpp
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+$(ODIR)/%.o: ../../src/modules/systemlib/%.cpp
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c
+	$(CC) -c -o $@ $< $(CFLAGS)
+
+#
+mixer_test: $(OBJ)
+	g++ -o $@ $^ $(CFLAGS) $(LIBS)
+
+.PHONY: clean
+
+clean:
+	rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~

Tools/tests-host/mixer_test.cpp

@@ -0,0 +1,12 @@
+#include <systemlib/mixer/mixer.h>
+#include <systemlib/err.h>
+#include "../../src/systemcmds/tests/tests.h"
+
+int main(int argc, char *argv[]) {
+	warnx("Host execution started");
+
+	char* args[] = {argv[0], "../../ROMFS/px4fmu_common/mixers/IO_pass.mix",
+				 "../../ROMFS/px4fmu_common/mixers/FMU_quad_w.mix"};
+
+	test_mixer(3, args);
+}

mavlink/include/mavlink/v1.0/mavlink_conversions.h

@@ -9,6 +9,10 @@
 #endif
 #include <math.h>
 
+#ifndef M_PI_2
+    #define M_PI_2 ((float)asin(1))
+#endif
+
 /**
  * @file mavlink_conversions.h
  *
@@ -49,12 +53,12 @@ MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, flo
     float phi, theta, psi;
     theta = asin(-dcm[2][0]);
 
-    if (fabs(theta - M_PI_2) < 1.0e-3f) {
+    if (fabsf(theta - (float)M_PI_2) < 1.0e-3f) {
         phi = 0.0f;
-        psi = (atan2(dcm[1][2] - dcm[0][1],
+        psi = (atan2f(dcm[1][2] - dcm[0][1],
                 dcm[0][2] + dcm[1][1]) + phi);
 
-    } else if (fabs(theta + M_PI_2) < 1.0e-3f) {
+    } else if (fabsf(theta + (float)M_PI_2) < 1.0e-3f) {
         phi = 0.0f;
         psi = atan2f(dcm[1][2] - dcm[0][1],
                   dcm[0][2] + dcm[1][1] - phi);

src/drivers/rgbled/rgbled.cpp

@@ -103,6 +103,7 @@ private:
 
 	bool			_running;
 	int			_led_interval;
+	bool			_should_run;
 	int			_counter;
 
 	void 			set_color(rgbled_color_t ledcolor);
@@ -136,6 +137,7 @@ RGBLED::RGBLED(int bus, int rgbled) :
 	_brightness(1.0f),
 	_running(false),
 	_led_interval(0),
+	_should_run(false),
 	_counter(0)
 {
 	memset(&_work, 0, sizeof(_work));
@@ -248,6 +250,11 @@ RGBLED::led_trampoline(void *arg)
 void
 RGBLED::led()
 {
+	if (!_should_run) {
+		_running = false;
+		return;
+	}
+
 	switch (_mode) {
 	case RGBLED_MODE_BLINK_SLOW:
 	case RGBLED_MODE_BLINK_NORMAL:
@@ -409,10 +416,10 @@ RGBLED::set_mode(rgbled_mode_t mode)
 {
 	if (mode != _mode) {
 		_mode = mode;
-		bool should_run = false;
 
 		switch (mode) {
 		case RGBLED_MODE_OFF:
+			_should_run = false;
 			send_led_enable(false);
 			break;
 
@@ -423,7 +430,7 @@ RGBLED::set_mode(rgbled_mode_t mode)
 			break;
 
 		case RGBLED_MODE_BLINK_SLOW:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_led_interval = 2000;
 			_brightness = 1.0f;
@@ -431,7 +438,7 @@ RGBLED::set_mode(rgbled_mode_t mode)
 			break;
 
 		case RGBLED_MODE_BLINK_NORMAL:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_led_interval = 500;
 			_brightness = 1.0f;
@@ -439,7 +446,7 @@ RGBLED::set_mode(rgbled_mode_t mode)
 			break;
 
 		case RGBLED_MODE_BLINK_FAST:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_led_interval = 100;
 			_brightness = 1.0f;
@@ -447,14 +454,14 @@ RGBLED::set_mode(rgbled_mode_t mode)
 			break;
 
 		case RGBLED_MODE_BREATHE:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_led_interval = 25;
 			send_led_enable(true);
 			break;
 
 		case RGBLED_MODE_PATTERN:
-			should_run = true;
+			_should_run = true;
 			_counter = 0;
 			_brightness = 1.0f;
 			send_led_enable(true);
@@ -466,16 +473,11 @@ RGBLED::set_mode(rgbled_mode_t mode)
 		}
 
 		/* if it should run now, start the workq */
-		if (should_run && !_running) {
+		if (_should_run && !_running) {
 			_running = true;
 			work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
 		}
 
-		/* if it should stop, then cancel the workq */
-		if (!should_run && _running) {
-			_running = false;
-			work_cancel(LPWORK, &_work);
-		}
 	}
 }
 

src/lib/ecl/l1/ecl_l1_pos_controller.cpp

@@ -130,8 +130,12 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	float alongTrackDist = vector_A_to_airplane * vector_AB;
 
 	/* estimate airplane position WRT to B */
-	math::Vector2f vector_B_to_airplane_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
+	math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
-	float bearing_wp_b = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX());
+	
+	/* calculate angle of airplane position vector relative to line) */
+
+	// XXX this could probably also be based solely on the dot product
+	float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB);
 
 	/* extension from [2], fly directly to A */
 	if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) {
@@ -148,21 +152,30 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 		/* bearing from current position to L1 point */
 		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
 
-// XXX this can be useful as last-resort guard, but is currently not needed
+	/*
-#if 0
+	 * If the AB vector and the vector from B to airplane point in the same
-	} else if (absf(bearing_wp_b) > math::radians(80.0f)) {
+	 * direction, we have missed the waypoint. At +- 90 degrees we are just passing it.
-		/* extension, fly back to waypoint */
+	 */
+	} else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) {
+		/*
+		 * Extension, fly back to waypoint.
+		 * 
+		 * This corner case is possible if the system was following
+		 * the AB line from waypoint A to waypoint B, then is
+		 * switched to manual mode (or otherwise misses the waypoint)
+		 * and behind the waypoint continues to follow the AB line.
+		 */
 
 		/* calculate eta to fly to waypoint B */
 		
 		/* velocity across / orthogonal to line */
-		xtrack_vel = ground_speed_vector % (-vector_B_to_airplane_unit);
+		xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit);
 		/* velocity along line */
-		ltrack_vel = ground_speed_vector * (-vector_B_to_airplane_unit);
+		ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
 		eta = atan2f(xtrack_vel, ltrack_vel);
 		/* bearing from current position to L1 point */
-		_nav_bearing = bearing_wp_b;
+		_nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX());
-#endif
+
 	} else {
 
 		/* calculate eta to fly along the line between A and B */

src/modules/sdlog2/sdlog2.c

@@ -583,6 +583,15 @@ int sdlog2_thread_main(int argc, char *argv[])
 		errx(1, "unable to create logging folder, exiting.");
 	}
 
+	const char *converter_in = "/etc/logging/conv.zip";
+	char* converter_out = malloc(120);
+	sprintf(converter_out, "%s/conv.zip", folder_path);
+
+	if (file_copy(converter_in, converter_out)) {
+		errx(1, "unable to copy conversion scripts, exiting.");
+	}
+	free(converter_out);
+
 	/* only print logging path, important to find log file later */
 	warnx("logging to directory: %s", folder_path);
 
@@ -1266,7 +1275,7 @@ int file_copy(const char *file_old, const char *file_new)
 	fclose(source);
 	fclose(target);
 
-	return ret;
+	return OK;
 }
 
 void handle_command(struct vehicle_command_s *cmd)

src/modules/systemlib/mixer/mixer.cpp

@@ -50,6 +50,8 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <ctype.h>
+#include <systemlib/err.h>
 
 #include "mixer.h"
 
@@ -114,6 +116,33 @@ Mixer::scale_check(struct mixer_scaler_s &scaler)
 	return 0;
 }
 
+const char *
+Mixer::findtag(const char *buf, unsigned &buflen, char tag)
+{
+	while (buflen >= 2) {
+		if ((buf[0] == tag) && (buf[1] == ':'))
+			return buf;
+		buf++;
+		buflen--;
+	}
+	return nullptr;
+}
+
+const char *
+Mixer::skipline(const char *buf, unsigned &buflen)
+{
+	const char *p;
+
+	/* if we can find a CR or NL in the buffer, skip up to it */
+	if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) {
+		/* skip up to it AND one beyond - could be on the NUL symbol now */
+		buflen -= (p - buf) + 1;
+		return p + 1;
+	}
+
+	return nullptr;
+}
+
 /****************************************************************************/
 
 NullMixer::NullMixer() :

src/modules/systemlib/mixer/mixer.h

@@ -128,7 +128,9 @@
 #ifndef _SYSTEMLIB_MIXER_MIXER_H
 #define _SYSTEMLIB_MIXER_MIXER_H value
 
+#include <nuttx/config.h>
 #include "drivers/drv_mixer.h"
+#include "mixer_load.h"
 
 /**
  * Abstract class defining a mixer mixing zero or more inputs to
@@ -210,6 +212,24 @@ protected:
 	 */
 	static int			scale_check(struct mixer_scaler_s &scaler);
 
+	/**
+	 * Find a tag
+	 *
+	 * @param buf			The buffer to operate on.
+	 * @param buflen		length of the buffer.
+	 * @param tag			character to search for.
+	 */
+	static const char *		findtag(const char *buf, unsigned &buflen, char tag);
+
+	/**
+	 * Skip a line
+	 *
+	 * @param buf			The buffer to operate on.
+	 * @param buflen		length of the buffer.
+	 * @return			0 / OK if a line could be skipped, 1 else
+	 */
+	static const char *		skipline(const char *buf, unsigned &buflen);
+
 private:
 };
 
@@ -238,6 +258,11 @@ public:
 	 */
 	void				reset();
 
+	/**
+	 * Count the mixers in the group.
+	 */
+	unsigned			count();
+
 	/**
 	 * Adds mixers to the group based on a text description in a buffer.
 	 *

src/modules/systemlib/mixer/mixer_group.cpp

@@ -111,6 +111,20 @@ MixerGroup::mix(float *outputs, unsigned space)
 	return index;
 }
 
+unsigned
+MixerGroup::count()
+{
+	Mixer	*mixer = _first;
+	unsigned index = 0;
+
+	while ((mixer != nullptr)) {
+		mixer = mixer->_next;
+		index++;
+	}
+
+	return index;
+}
+
 void
 MixerGroup::groups_required(uint32_t &groups)
 {
@@ -170,6 +184,7 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
 
 			/* only adjust buflen if parsing was successful */
 			buflen = resid;
+			debug("SUCCESS - buflen: %d", buflen);
 		} else {
 
 			/*

src/modules/systemlib/mixer/mixer_load.c

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mixer_load.c
+ *
+ * Programmable multi-channel mixer library.
+ */
+
+#include <nuttx/config.h>
+#include <string.h>
+#include <stdio.h>
+#include <ctype.h>
+
+#include "mixer_load.h"
+
+int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
+{
+	FILE		*fp;
+	char		line[120];
+
+	/* open the mixer definition file */
+	fp = fopen(fname, "r");
+	if (fp == NULL) {
+		return 1;
+	}
+
+	/* read valid lines from the file into a buffer */
+	buf[0] = '\0';
+	for (;;) {
+
+		/* get a line, bail on error/EOF */
+		line[0] = '\0';
+		if (fgets(line, sizeof(line), fp) == NULL)
+			break;
+
+		/* if the line doesn't look like a mixer definition line, skip it */
+		if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
+			continue;
+
+		/* compact whitespace in the buffer */
+		char *t, *f;
+		for (f = line; *f != '\0'; f++) {
+			/* scan for space characters */
+			if (*f == ' ') {
+				/* look for additional spaces */
+				t = f + 1;
+				while (*t == ' ')
+					t++;
+				if (*t == '\0') {
+					/* strip trailing whitespace */
+					*f = '\0';
+				} else if (t > (f + 1)) {
+					memmove(f + 1, t, strlen(t) + 1);
+				}
+			}
+		}
+
+		/* if the line is too long to fit in the buffer, bail */
+		if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
+			return 1;
+		}
+
+		/* add the line to the buffer */
+		strcat(buf, line);
+	}
+
+	return 0;
+}
+

src/modules/systemlib/mixer/mixer_load.h

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mixer_load.h
+ *
+ */
+
+
+#ifndef _SYSTEMLIB_MIXER_LOAD_H
+#define _SYSTEMLIB_MIXER_LOAD_H value
+
+#include <nuttx/config.h>
+
+__BEGIN_DECLS
+
+__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen);
+
+__END_DECLS
+
+#endif

src/modules/systemlib/mixer/mixer_multirotor.cpp

@@ -130,7 +130,7 @@ const MultirotorMixer::Rotor _config_octa_plus[] = {
 	{  1.000000,  0.000000, -1.00 },
 	{ -1.000000,  0.000000, -1.00 },
 };
-const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
+const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
 	&_config_quad_x[0],
 	&_config_quad_plus[0],
 	&_config_quad_v[0],
@@ -140,7 +140,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOME
 	&_config_octa_x[0],
 	&_config_octa_plus[0],
 };
-const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
+const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
 	4, /* quad_x */
 	4, /* quad_plus */
 	4, /* quad_v */
@@ -205,11 +205,17 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	}
 
 	if (used > (int)buflen) {
-		debug("multirotor spec used %d of %u", used, buflen);
+		debug("OVERFLOW: multirotor spec used %d of %u", used, buflen);
 		return nullptr;
 	}
 
-	buflen -= used;
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return nullptr;
+	}
+
+	debug("remaining in buf: %d, first char: %c", buflen, buf[0]);
 
 	if (!strcmp(geomname, "4+")) {
 		geometry = MultirotorMixer::QUAD_PLUS;

src/modules/systemlib/mixer/mixer_simple.cpp

@@ -55,7 +55,7 @@
 #include "mixer.h"
 
 #define debug(fmt, args...)	do { } while(0)
-// #define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
+//#define debug(fmt, args...)	do { printf("[mixer] " fmt "\n", ##args); } while(0)
 
 SimpleMixer::SimpleMixer(ControlCallback control_cb,
 			 uintptr_t cb_handle,
@@ -71,28 +71,6 @@ SimpleMixer::~SimpleMixer()
 		free(_info);
 }
 
-static const char *
-findtag(const char *buf, unsigned &buflen, char tag)
-{
-	while (buflen >= 2) {
-		if ((buf[0] == tag) && (buf[1] == ':'))
-			return buf;
-		buf++;
-		buflen--;
-	}
-	return nullptr;
-}
-
-static void
-skipline(const char *buf, unsigned &buflen)
-{
-	const char *p;
-
-	/* if we can find a CR or NL in the buffer, skip up to it */
-	if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen)))
-		buflen -= (p - buf);
-}
-
 int
 SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler)
 {
@@ -111,7 +89,12 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
 		debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n);
 		return -1;
 	}
-	skipline(buf, buflen);
+
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return -1;
+	}
 
 	scaler.negative_scale	= s[0] / 10000.0f;
 	scaler.positive_scale	= s[1] / 10000.0f;
@@ -130,7 +113,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
 
 	buf = findtag(buf, buflen, 'S');
 	if ((buf == nullptr) || (buflen < 16)) {
-		debug("contorl parser failed finding tag, ret: '%s'", buf);
+		debug("control parser failed finding tag, ret: '%s'", buf);
 		return -1;
 	}
 
@@ -139,7 +122,12 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
 		debug("control parse failed on '%s'", buf);
 		return -1;
 	}
-	skipline(buf, buflen);
+
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		return -1;
+	}
 
 	control_group		= u[0];
 	control_index		= u[1];
@@ -161,29 +149,17 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
 	int used;
 	const char *end = buf + buflen;
 
-	/* enforce that the mixer ends with space or a new line */
-	for (int i = buflen - 1; i >= 0; i--) {
-		if (buf[i] == '\0')
-			continue;
-
-		/* require a space or newline at the end of the buffer, fail on printable chars */
-		if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
-			/* found a line ending or space, so no split symbols / numbers. good. */
-			break;
-		} else {
-			debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]);
-			goto out;
-		}
-
-	}
-
 	/* get the base info for the mixer */
 	if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
 		debug("simple parse failed on '%s'", buf);
 		goto out;
 	}
 
-	buflen -= used;
+	buf = skipline(buf, buflen);
+	if (buf == nullptr) {
+		debug("no line ending, line is incomplete");
+		goto out;
+	}
 
 	mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
 

src/modules/systemlib/mixer/module.mk

@@ -39,4 +39,5 @@ LIBNAME	= mixerlib
 SRCS		= mixer.cpp \
 		  mixer_group.cpp \
 		  mixer_multirotor.cpp \
-		  mixer_simple.cpp
+		  mixer_simple.cpp \
+		  mixer_load.c

src/systemcmds/mixer/mixer.cpp

@@ -47,10 +47,15 @@
 #include <nuttx/compiler.h>
 
 #include <systemlib/err.h>
-#include <drivers/drv_mixer.h>
+#include <systemlib/mixer/mixer.h>
 #include <uORB/topics/actuator_controls.h>
 
-__EXPORT int mixer_main(int argc, char *argv[]);
+/**
+ * Mixer utility for loading mixer files to devices
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int mixer_main(int argc, char *argv[]);
 
 static void	usage(const char *reason) noreturn_function;
 static void	load(const char *devname, const char *fname) noreturn_function;
@@ -87,8 +92,6 @@ static void
 load(const char *devname, const char *fname)
 {
 	int		dev;
-	FILE		*fp;
-	char		line[120];
 	char		buf[2048];
 
 	/* open the device */
@@ -99,49 +102,7 @@ load(const char *devname, const char *fname)
 	if (ioctl(dev, MIXERIOCRESET, 0))
 		err(1, "can't reset mixers on %s", devname);
 
-	/* open the mixer definition file */
+	load_mixer_file(fname, &buf[0], sizeof(buf));
-	fp = fopen(fname, "r");
-	if (fp == NULL)
-		err(1, "can't open %s", fname);
-
-	/* read valid lines from the file into a buffer */
-	buf[0] = '\0';
-	for (;;) {
-
-		/* get a line, bail on error/EOF */
-		line[0] = '\0';
-		if (fgets(line, sizeof(line), fp) == NULL)
-			break;
-
-		/* if the line doesn't look like a mixer definition line, skip it */
-		if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
-			continue;
-
-		/* compact whitespace in the buffer */
-		char *t, *f;
-		for (f = buf; *f != '\0'; f++) {
-			/* scan for space characters */
-			if (*f == ' ') {
-				/* look for additional spaces */
-				t = f + 1;
-				while (*t == ' ')
-					t++;
-				if (*t == '\0') {
-					/* strip trailing whitespace */
-					*f = '\0';
-				} else if (t > (f + 1)) {
-					memmove(f + 1, t, strlen(t) + 1);
-				}
-			}
-		}
-
-		/* if the line is too long to fit in the buffer, bail */
-		if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf))
-			break;
-
-		/* add the line to the buffer */
-		strcat(buf, line);
-	}
 
 	/* XXX pass the buffer to the device */
 	int ret = ioctl(dev, MIXERIOCLOADBUF, (unsigned long)buf);

src/systemcmds/mixer/module.mk

@@ -36,7 +36,7 @@
 #
 
 MODULE_COMMAND	 = mixer
-SRCS		 = mixer.c
+SRCS		 = mixer.cpp
 
 MODULE_STACKSIZE = 4096
 

src/systemcmds/tests/module.mk

@@ -23,6 +23,7 @@ SRCS			 = test_adc.c \
 			   test_uart_console.c \
 			   test_uart_loopback.c \
 			   test_uart_send.c \
+			   test_mixer.cpp \
 			   tests_file.c \
 			   tests_main.c \
 			   tests_param.c

src/systemcmds/tests/test_mixer.cpp

@@ -0,0 +1,199 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_mixer.hpp
+ *
+ * Mixer load test
+ */
+
+#include <nuttx/config.h>
+
+#include <sys/types.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <string.h>
+#include <time.h>
+
+#include <systemlib/err.h>
+#include <systemlib/mixer/mixer.h>
+
+#include "tests.h"
+
+static int	mixer_callback(uintptr_t handle,
+			       uint8_t control_group,
+			       uint8_t control_index,
+			       float &control);
+
+int test_mixer(int argc, char *argv[])
+{
+	warnx("testing mixer");
+
+	char *filename = "/etc/mixers/IO_pass.mix";
+
+	if (argc > 1)
+		filename = argv[1];
+
+	warnx("loading: %s", filename);
+
+	char		buf[2048];
+
+	load_mixer_file(filename, &buf[0], sizeof(buf));
+	unsigned loaded = strlen(buf);
+
+	warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
+
+	/* load the mixer in chunks, like
+	 * in the case of a remote load,
+	 * e.g. on PX4IO.
+	 */
+
+	unsigned nused = 0;
+
+	const unsigned chunk_size = 64;
+
+	MixerGroup mixer_group(mixer_callback, 0);
+
+	/* load at once test */
+	unsigned xx = loaded;
+	mixer_group.load_from_buf(&buf[0], xx);
+	warnx("complete buffer load: loaded %u mixers", mixer_group.count());
+	if (mixer_group.count() != 8)
+		return 1;
+
+	unsigned empty_load = 2;
+	char empty_buf[2];
+	empty_buf[0] = ' ';
+	empty_buf[1] = '\0';
+	mixer_group.reset();
+	mixer_group.load_from_buf(&empty_buf[0], empty_load);
+	warnx("empty buffer load: loaded %u mixers, used: %u", mixer_group.count(), empty_load);
+	if (empty_load != 0)
+		return 1;
+
+	/* FIRST mark the mixer as invalid */
+	bool mixer_ok = false;
+	/* THEN actually delete it */
+	mixer_group.reset();
+	char mixer_text[256];		/* large enough for one mixer */
+	unsigned mixer_text_length = 0;
+
+	unsigned transmitted = 0;
+
+	warnx("transmitted: %d, loaded: %d", transmitted, loaded);
+
+	while (transmitted < loaded) {
+
+		unsigned	text_length = (loaded - transmitted > chunk_size) ? chunk_size : loaded - transmitted;
+
+		/* check for overflow - this would be really fatal */
+		if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
+			bool mixer_ok = false;
+			return 1;
+		}
+
+		/* append mixer text and nul-terminate */
+		memcpy(&mixer_text[mixer_text_length], &buf[transmitted], text_length);
+		mixer_text_length += text_length;
+		mixer_text[mixer_text_length] = '\0';
+		warnx("buflen %u, text:\n\"%s\"", mixer_text_length, &mixer_text[0]);
+
+		/* process the text buffer, adding new mixers as their descriptions can be parsed */
+		unsigned resid = mixer_text_length;
+		mixer_group.load_from_buf(&mixer_text[0], resid);
+
+		/* if anything was parsed */
+		if (resid != mixer_text_length) {
+
+			/* only set mixer ok if no residual is left over */
+			if (resid == 0) {
+				mixer_ok = true;
+			} else {
+				/* not yet reached the end of the mixer, set as not ok */
+				mixer_ok = false;
+			}
+
+			warnx("used %u", mixer_text_length - resid);
+
+			/* copy any leftover text to the base of the buffer for re-use */
+			if (resid > 0)
+				memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid);
+
+			mixer_text_length = resid;
+		}
+
+		transmitted += text_length;
+	}
+
+	warnx("chunked load: loaded %u mixers", mixer_group.count());
+
+	if (mixer_group.count() != 8)
+		return 1;
+
+	/* load multirotor at once test */
+	mixer_group.reset();
+
+	if (argc > 2)
+		filename = argv[2];
+	else
+		filename = "/etc/mixers/FMU_quad_w.mix";
+
+	load_mixer_file(filename, &buf[0], sizeof(buf));
+	loaded = strlen(buf);
+
+	warnx("loaded: \n\"%s\"\n (%d chars)", &buf[0], loaded);
+
+	unsigned mc_loaded = loaded;
+	mixer_group.load_from_buf(&buf[0], mc_loaded);
+	warnx("complete buffer load: loaded %u mixers", mixer_group.count());
+	if (mixer_group.count() != 8)
+		return 1;
+}
+
+static int
+mixer_callback(uintptr_t handle,
+	       uint8_t control_group,
+	       uint8_t control_index,
+	       float &control)
+{
+	if (control_group != 0)
+		return -1;
+
+	control = 0.0f;
+
+	return 0;
+}

src/systemcmds/tests/tests.h

@@ -76,6 +76,8 @@
  * Public Function Prototypes
  ****************************************************************************/
 
+__BEGIN_DECLS
+
 extern int	test_sensors(int argc, char *argv[]);
 extern int	test_gpio(int argc, char *argv[]);
 extern int	test_hrt(int argc, char *argv[]);
@@ -98,5 +100,8 @@ extern int	test_jig_voltages(int argc, char *argv[]);
 extern int	test_param(int argc, char *argv[]);
 extern int	test_bson(int argc, char *argv[]);
 extern int	test_file(int argc, char *argv[]);
+extern int	test_mixer(int argc, char *argv[]);
+
+__END_DECLS
 
 #endif /* __APPS_PX4_TESTS_H */

src/systemcmds/tests/tests_main.c

@@ -110,6 +110,7 @@ const struct {
 	{"param",		test_param,	0},
 	{"bson",		test_bson,	0},
 	{"file",		test_file,	0},
+	{"mixer",		test_mixer,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
 	{NULL,			NULL, 		0}
 };