Merge branch 'master' into export-build

6 files changed, 482 insertions, 1 deletions

diff --git a/apps/systemcmds/i2c/Makefile b/apps/systemcmds/i2c/Makefile
new file mode 100644
index 000000000..046e74757
--- /dev/null
+++ b/apps/systemcmds/i2c/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Build the i2c test tool.
+#
+
+APPNAME		 = i2c
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 4096
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/systemcmds/i2c/i2c.c b/apps/systemcmds/i2c/i2c.c
new file mode 100644
index 000000000..e1babdc12
--- /dev/null
+++ b/apps/systemcmds/i2c/i2c.c
@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 i2c.c
+ *
+ * i2c hacking tool
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/mount.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+
+#include <nuttx/i2c.h>
+
+#include <arch/board/board.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/err.h"
+
+#ifndef PX4_I2C_BUS_ONBOARD
+#  error PX4_I2C_BUS_ONBOARD not defined, no device interface
+#endif
+#ifndef PX4_I2C_OBDEV_PX4IO
+#  error PX4_I2C_OBDEV_PX4IO not defined
+#endif
+
+__EXPORT int i2c_main(int argc, char *argv[]);
+
+static int transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len);
+
+static struct i2c_dev_s *i2c;
+
+int i2c_main(int argc, char *argv[])
+{
+	/* find the right I2C */
+	i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
+
+	if (i2c == NULL)
+		errx(1, "failed to locate I2C bus");
+
+	usleep(100000);
+
+	uint8_t buf[] = { 0, 4};
+	int ret = transfer(PX4_I2C_OBDEV_PX4IO, buf, sizeof(buf), NULL, 0);
+
+	if (ret)
+		errx(1, "send failed - %d", ret);
+
+	uint32_t val;
+	ret = transfer(PX4_I2C_OBDEV_PX4IO, NULL, 0, (uint8_t *)&val, sizeof(val));
+	if (ret)
+		errx(1, "recive failed - %d", ret);
+
+	errx(0, "got 0x%08x", val);
+}
+
+static int
+transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
+{
+	struct i2c_msg_s msgv[2];
+	unsigned msgs;
+	int ret;
+
+	//	debug("transfer out %p/%u  in %p/%u", send, send_len, recv, recv_len);
+
+	msgs = 0;
+
+	if (send_len > 0) {
+		msgv[msgs].addr = address;
+		msgv[msgs].flags = 0;
+		msgv[msgs].buffer = send;
+		msgv[msgs].length = send_len;
+		msgs++;
+	}
+
+	if (recv_len > 0) {
+		msgv[msgs].addr = address;
+		msgv[msgs].flags = I2C_M_READ;
+		msgv[msgs].buffer = recv;
+		msgv[msgs].length = recv_len;
+		msgs++;
+	}
+
+	if (msgs == 0)
+		return -1;
+
+	/*
+	 * I2C architecture means there is an unavoidable race here
+	 * if there are any devices on the bus with a different frequency
+	 * preference.  Really, this is pointless.
+	 */
+	I2C_SETFREQUENCY(i2c, 400000);
+	ret = I2C_TRANSFER(i2c, &msgv[0], msgs);
+
+	// reset the I2C bus to unwedge on error
+	if (ret != OK)
+		up_i2creset(i2c);
+
+	return ret;
+}
diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c
index e2f7b5bd5..55c4f0836 100644
--- a/apps/systemcmds/mixer/mixer.c
+++ b/apps/systemcmds/mixer/mixer.c
@@ -117,7 +117,23 @@ load(const char *devname, const char *fname)
 		if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
 			continue;
 
-		/* XXX an optimisation here would be to strip extra whitespace */
+		/* 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))
diff --git a/apps/systemcmds/preflight_check/preflight_check.c b/apps/systemcmds/preflight_check/preflight_check.c
index f25f812ae..9ac6f0b5e 100644
--- a/apps/systemcmds/preflight_check/preflight_check.c
+++ b/apps/systemcmds/preflight_check/preflight_check.c
@@ -54,6 +54,8 @@
 #include <drivers/drv_accel.h>
 #include <drivers/drv_baro.h>
 
+#include <mavlink/mavlink_log.h>
+
 __EXPORT int preflight_check_main(int argc, char *argv[]);
 static int led_toggle(int leds, int led);
 static int led_on(int leds, int led);
@@ -75,6 +77,8 @@ int preflight_check_main(int argc, char *argv[])
 	bool system_ok = true;
 
 	int fd;
+	/* open text message output path */
+	int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 	int ret;
 
 	/* give the system some time to sample the sensors in the background */
@@ -86,6 +90,7 @@ int preflight_check_main(int argc, char *argv[])
 	fd = open(MAG_DEVICE_PATH, 0);
 	if (fd < 0) {
 		warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG");
 		system_ok = false;
 		goto system_eval;
 	}
@@ -93,6 +98,7 @@ int preflight_check_main(int argc, char *argv[])
 	
 	if (ret != OK) {
 		warnx("magnetometer calibration missing or bad - calibrate magnetometer first");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CALIBRATION");
 		system_ok = false;
 		goto system_eval;
 	}
@@ -105,6 +111,7 @@ int preflight_check_main(int argc, char *argv[])
 	
 	if (ret != OK) {
 		warnx("accel self test failed");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK");
 		system_ok = false;
 		goto system_eval;
 	}
@@ -117,6 +124,7 @@ int preflight_check_main(int argc, char *argv[])
 	
 	if (ret != OK) {
 		warnx("gyro self test failed");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK");
 		system_ok = false;
 		goto system_eval;
 	}
diff --git a/apps/systemcmds/pwm/Makefile b/apps/systemcmds/pwm/Makefile
new file mode 100644
index 000000000..5ab105ed1
--- /dev/null
+++ b/apps/systemcmds/pwm/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Build the pwm tool.
+#
+
+APPNAME		 = pwm
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 4096
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/systemcmds/pwm/pwm.c b/apps/systemcmds/pwm/pwm.c
new file mode 100644
index 000000000..de7a53199
--- /dev/null
+++ b/apps/systemcmds/pwm/pwm.c
@@ -0,0 +1,233 @@
+/****************************************************************************
+ *
+ *   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 pwm.c
+ *
+ * PWM servo output configuration and monitoring tool.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/mount.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+
+#include <nuttx/i2c.h>
+#include <nuttx/mtd.h>
+#include <nuttx/fs/nxffs.h>
+#include <nuttx/fs/ioctl.h>
+
+#include <arch/board/board.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/err.h"
+#include "drivers/drv_pwm_output.h"
+
+static void	usage(const char *reason);
+__EXPORT int	pwm_main(int argc, char *argv[]);
+
+
+static void
+usage(const char *reason)
+{
+	if (reason != NULL)
+		warnx("%s", reason);
+	errx(1, 
+		"usage:\n"
+		"pwm [-v] [-d <device>] [-u <alt_rate>] [-c <channel group>] [arm|disarm] [<channel_value> ...]\n"
+		"  -v                 Print information about the PWM device\n"
+		"  <device>           PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
+		"  <alt_rate>         PWM update rate for channels in <alt_channel_mask>\n"
+		"  <channel_group>    Channel group that should update at the alternate rate (may be specified more than once)\n"
+		"  arm | disarm       Arm or disarm the ouptut\n"
+		"  <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n"
+		"\n"
+		"When -c is specified, any channel groups not listed with -c will update at the default rate.\n"
+		);
+
+}
+
+int
+pwm_main(int argc, char *argv[])
+{
+	const char *dev = PWM_OUTPUT_DEVICE_PATH;
+	unsigned alt_rate = 0;
+	uint32_t alt_channel_groups = 0;
+	bool alt_channels_set = false;
+	bool print_info = false;
+	int ch;
+	int ret;
+	char *ep;
+	unsigned group;
+
+	if (argc < 2)
+		usage(NULL);
+
+	while ((ch = getopt(argc, argv, "c:d:u:v")) != EOF) {
+		switch (ch) {
+		case 'c':
+			group = strtoul(optarg, &ep, 0);
+			if ((*ep != '\0') || (group >= 32))
+				usage("bad channel_group value");
+			alt_channel_groups |= (1 << group);
+			alt_channels_set = true;
+			break;
+
+		case 'd':
+			dev = optarg;
+			break;
+
+		case 'u':
+			alt_rate = strtol(optarg, &ep, 0);
+			if (*ep != '\0')
+				usage("bad alt_rate value");
+			break;
+
+		case 'v':
+			print_info = true;
+			break;
+
+		default:
+			usage(NULL);
+		}
+	}
+	argc -= optind;
+	argv += optind;
+
+	/* open for ioctl only */
+	int fd = open(dev, 0);
+	if (fd < 0)
+		err(1, "can't open %s", dev);
+
+	/* change alternate PWM rate */
+	if (alt_rate > 0) {
+		ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate);
+		if (ret != OK)
+			err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
+	}
+
+	/* assign alternate rate to channel groups */
+	if (alt_channels_set) {
+		uint32_t mask = 0;
+
+		for (unsigned group = 0; group < 32; group++) {
+			if ((1 << group) & alt_channel_groups) {
+				uint32_t group_mask;
+
+				ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
+				if (ret != OK)
+					err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
+
+				mask |= group_mask;
+			}
+		}
+
+		ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask);
+		if (ret != OK)
+			err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
+	}
+
+	/* iterate remaining arguments */
+	unsigned channel = 0;
+	while (argc--) {
+		const char *arg = argv[0];
+		argv++;
+		if (!strcmp(arg, "arm")) {
+			ret = ioctl(fd, PWM_SERVO_ARM, 0);
+			if (ret != OK)
+				err(1, "PWM_SERVO_ARM");
+			continue;
+		}
+		if (!strcmp(arg, "disarm")) {
+			ret = ioctl(fd, PWM_SERVO_DISARM, 0);
+			if (ret != OK)
+				err(1, "PWM_SERVO_DISARM");
+			continue;
+		}
+		unsigned pwm_value = strtol(arg, &ep, 0);
+		if (*ep == '\0') {
+			ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value);
+			if (ret != OK)
+				err(1, "PWM_SERVO_SET(%d)", channel);
+			channel++;
+			continue;
+		}
+		usage("unrecognised option");
+	}
+
+	/* print verbose info */
+	if (print_info) {
+		/* get the number of servo channels */
+		unsigned count;
+		ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count);
+		if (ret != OK)
+			err(1, "PWM_SERVO_GET_COUNT");
+
+		/* print current servo values */
+		for (unsigned i = 0; i < count; i++) {
+			servo_position_t spos;
+
+			ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos);
+			if (ret == OK) {
+				printf("channel %u: %uus\n", i, spos);
+			} else {
+				printf("%u: ERROR\n", i);
+			}
+		}
+
+		/* print rate groups */
+		for (unsigned i = 0; i < count; i++) {
+			uint32_t group_mask;
+
+			ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask);
+			if (ret != OK)
+				break;
+			if (group_mask != 0) {
+				printf("channel group %u: channels", i);
+				for (unsigned j = 0; j < 32; j++)
+					if (group_mask & (1 << j))
+						printf(" %u", j);
+				printf("\n");
+			}
+		}
+		fflush(stdout);
+	}
+	exit(0);
+}
\ No newline at end of file