Merged master into st24

src/drivers/gps/ubx.cpp

@@ -189,6 +189,18 @@ UBX::configure(unsigned &baudrate)
 		return 1;
 	}
 
+#ifdef UBX_CONFIGURE_SBAS
+	/* send a SBAS message to set the SBAS options */
+	memset(&_buf.payload_tx_cfg_sbas, 0, sizeof(_buf.payload_tx_cfg_sbas));
+	_buf.payload_tx_cfg_sbas.mode		= UBX_TX_CFG_SBAS_MODE;
+
+	send_message(UBX_MSG_CFG_SBAS, _buf.raw, sizeof(_buf.payload_tx_cfg_sbas));
+
+	if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) {
+		return 1;
+	}
+#endif
+
 	/* configure message rates */
 	/* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */
 

src/drivers/gps/ubx.h

@@ -73,6 +73,7 @@
 #define UBX_ID_CFG_MSG		0x01
 #define UBX_ID_CFG_RATE		0x08
 #define UBX_ID_CFG_NAV5		0x24
+#define UBX_ID_CFG_SBAS		0x16
 #define UBX_ID_MON_VER		0x04
 #define UBX_ID_MON_HW		0x09
 
@@ -89,6 +90,7 @@
 #define UBX_MSG_CFG_MSG		((UBX_CLASS_CFG) | UBX_ID_CFG_MSG << 8)
 #define UBX_MSG_CFG_RATE	((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8)
 #define UBX_MSG_CFG_NAV5	((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8)
+#define UBX_MSG_CFG_SBAS	((UBX_CLASS_CFG) | UBX_ID_CFG_SBAS << 8)
 #define UBX_MSG_MON_HW		((UBX_CLASS_MON) | UBX_ID_MON_HW << 8)
 #define UBX_MSG_MON_VER		((UBX_CLASS_MON) | UBX_ID_MON_VER << 8)
 
@@ -128,6 +130,11 @@
 #define UBX_TX_CFG_NAV5_DYNMODEL	7		/**< 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */
 #define UBX_TX_CFG_NAV5_FIXMODE		2		/**< 1 2D only, 2 3D only, 3 Auto 2D/3D */
 
+/* TX CFG-SBAS message contents */
+#define UBX_TX_CFG_SBAS_MODE_ENABLED	1				/**< SBAS enabled */
+#define UBX_TX_CFG_SBAS_MODE_DISABLED	0				/**< SBAS disabled */
+#define UBX_TX_CFG_SBAS_MODE		UBX_TX_CFG_SBAS_MODE_DISABLED	/**< SBAS enabled or disabled */
+
 /* TX CFG-MSG message contents */
 #define UBX_TX_CFG_MSG_RATE1_5HZ	0x01 		/**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
 #define UBX_TX_CFG_MSG_RATE1_1HZ	0x05		/**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
@@ -383,6 +390,15 @@ typedef struct {
 	uint32_t	reserved4;
 } ubx_payload_tx_cfg_nav5_t;
 
+/* tx cfg-sbas */
+typedef struct {
+	uint8_t		mode;
+	uint8_t		usage;
+	uint8_t		maxSBAS;
+	uint8_t		scanmode2;
+	uint32_t	scanmode1;
+} ubx_payload_tx_cfg_sbas_t;
+
 /* Tx CFG-MSG */
 typedef struct {
 	union {
@@ -413,6 +429,7 @@ typedef union {
 	ubx_payload_tx_cfg_prt_t		payload_tx_cfg_prt;
 	ubx_payload_tx_cfg_rate_t		payload_tx_cfg_rate;
 	ubx_payload_tx_cfg_nav5_t		payload_tx_cfg_nav5;
+	ubx_payload_tx_cfg_sbas_t		payload_tx_cfg_sbas;
 	ubx_payload_tx_cfg_msg_t		payload_tx_cfg_msg;
 	uint8_t					raw[];
 } ubx_buf_t;

src/drivers/hil/hil.cpp

@@ -392,7 +392,8 @@ HIL::task_main()
 		if (fds[0].revents & POLLIN) {
 
 			/* get controls - must always do this to avoid spinning */
-			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
+			orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+				     ORB_ID(actuator_controls_1), _t_actuators, &_controls);
 
 			/* can we mix? */
 			if (_mixers != nullptr) {

src/drivers/sf0x/module.mk

@@ -37,6 +37,7 @@
 
 MODULE_COMMAND	= sf0x
 
-SRCS		= sf0x.cpp
+SRCS		= sf0x.cpp \
+		  sf0x_parser.cpp
 
 MAXOPTIMIZATION	 = -Os

src/drivers/sf0x/sf0x.cpp

@@ -72,6 +72,8 @@
 
 #include <board_config.h>
 
+#include "sf0x_parser.h"
+
 /* Configuration Constants */
 
 /* oddly, ERROR is not defined for c++ */
@@ -120,6 +122,7 @@ private:
 	int				_fd;
 	char				_linebuf[10];
 	unsigned			_linebuf_index;
+	enum SF0X_PARSE_STATE		_parse_state;
 	hrt_abstime			_last_read;
 
 	orb_advert_t			_range_finder_topic;
@@ -186,6 +189,7 @@ SF0X::SF0X(const char *port) :
 	_collect_phase(false),
 	_fd(-1),
 	_linebuf_index(0),
+	_parse_state(SF0X_PARSE_STATE0_UNSYNC),
 	_last_read(0),
 	_range_finder_topic(-1),
 	_consecutive_fail_count(0),
@@ -200,12 +204,6 @@ SF0X::SF0X(const char *port) :
 		warnx("FAIL: laser fd");
 	}
 
-	/* tell it to stop auto-triggering */
-	char stop_auto = ' ';
-	(void)::write(_fd, &stop_auto, 1);
-	usleep(100);
-	(void)::write(_fd, &stop_auto, 1);
-
 	struct termios uart_config;
 
 	int termios_state;
@@ -520,20 +518,15 @@ SF0X::collect()
 	/* clear buffer if last read was too long ago */
 	uint64_t read_elapsed = hrt_elapsed_time(&_last_read);
 
-	/* timed out - retry */
-	if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
-		_linebuf_index = 0;
-	}
-
 	/* the buffer for read chars is buflen minus null termination */
-	unsigned readlen = sizeof(_linebuf) - 1;
+	char readbuf[sizeof(_linebuf)];
+	unsigned readlen = sizeof(readbuf) - 1;
 
 	/* read from the sensor (uart buffer) */
-	ret = ::read(_fd, &_linebuf[_linebuf_index], readlen - _linebuf_index);
+	ret = ::read(_fd, &readbuf[0], readlen);
 
 	if (ret < 0) {
-		_linebuf[sizeof(_linebuf) - 1] = '\0';
-		debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf);
+		debug("read err: %d", ret);
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
 
@@ -548,84 +541,23 @@ SF0X::collect()
 		return -EAGAIN;
 	}
 
-	/* let the write pointer point to the next free entry */
-	_linebuf_index += ret;
-
 	_last_read = hrt_absolute_time();
 
-	/* require a reasonable amount of minimum bytes */
-	if (_linebuf_index < 6) {
-		/* we need at this format: x.xx\r\n */
-		return -EAGAIN;
-
-	} else if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') {
-
-		if (_linebuf_index == readlen) {
-			/* we have a full buffer, but no line ending - abort */
-			_linebuf_index = 0;
-			perf_count(_comms_errors);
-			return -ENOMEM;
-		} else {
-			/* incomplete read, reschedule ourselves */
-			return -EAGAIN;
+	float si_units;
+	bool valid = false;
+	
+	for (unsigned i = 0; i < ret; i++) {
+		if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) {
+			valid = true;
 		}
 	}
 
-	char *end;
-	float si_units;
-	bool valid;
-
-	/* enforce line ending */
-	_linebuf[_linebuf_index] = '\0';
-
-	if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
-		si_units = -1.0f;
-		valid = false;
-
-	} else {
-
-		/* we need to find a dot in the string, as we're missing the meters part else */
-		valid = false;
-
-		/* wipe out partially read content from last cycle(s), check for dot */
-		for (unsigned i = 0; i < (_linebuf_index - 2); i++) {
-			if (_linebuf[i] == '\n') {
-				/* wipe out any partial measurements */
-				for (unsigned j = 0; j <= i; j++) {
-					_linebuf[j] = ' ';
-				}
-			}
-
-			/* we need a digit before the dot and a dot for a valid number */
-			if (i > 0 && ((_linebuf[i - 1] >= '0') && (_linebuf[i - 1] <= '9')) && (_linebuf[i] == '.')) {
-				valid = true;
-			}
-		}
-
-		if (valid) {
-			si_units = strtod(_linebuf, &end);
-
-			/* we require at least four characters for a valid number */
-			if (end > _linebuf + 3) {
-				valid = true;
-			} else {
-				si_units = -1.0f;
-				valid = false;
-			}
-		}
+	if (!valid) {
+		return -EAGAIN;
 	}
 
 	debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));
 
-	/* done with this chunk, resetting - even if invalid */
-	_linebuf_index = 0;
-
-	/* if its invalid, there is no reason to forward the value */
-	if (!valid) {
-		perf_count(_comms_errors);
-		return -EINVAL;
-	}
-
 	struct range_finder_report report;
 
 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */

src/drivers/sf0x/sf0x_parser.cpp

@@ -0,0 +1,155 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 sf0x_parser.cpp
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Driver for the Lightware SF0x laser rangefinder series
+ */
+
+#include "sf0x_parser.h"
+#include <string.h>
+#include <stdlib.h>
+
+//#define SF0X_DEBUG
+
+#ifdef SF0X_DEBUG
+#include <stdio.h>
+
+const char *parser_state[] = {
+	"0_UNSYNC",
+	"1_SYNC",
+	"2_GOT_DIGIT0",
+	"3_GOT_DOT",
+	"4_GOT_DIGIT1",
+	"5_GOT_DIGIT2",
+	"6_GOT_CARRIAGE_RETURN"
+};
+#endif
+
+int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist)
+{
+	int ret = -1;
+	char *end;
+
+	switch (*state) {
+	case SF0X_PARSE_STATE0_UNSYNC:
+		if (c == '\n') {
+			*state = SF0X_PARSE_STATE1_SYNC;
+			(*parserbuf_index) = 0;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE1_SYNC:
+		if (c >= '0' && c <= '9') {
+			*state = SF0X_PARSE_STATE2_GOT_DIGIT0;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE2_GOT_DIGIT0:
+		if (c >= '0' && c <= '9') {
+			*state = SF0X_PARSE_STATE2_GOT_DIGIT0;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+
+		} else if (c == '.') {
+			*state = SF0X_PARSE_STATE3_GOT_DOT;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE3_GOT_DOT:
+		if (c >= '0' && c <= '9') {
+			*state = SF0X_PARSE_STATE4_GOT_DIGIT1;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE4_GOT_DIGIT1:
+		if (c >= '0' && c <= '9') {
+			*state = SF0X_PARSE_STATE5_GOT_DIGIT2;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE5_GOT_DIGIT2:
+		if (c == '\r') {
+			*state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN:
+		if (c == '\n') {
+			parserbuf[*parserbuf_index] = '\0';
+			*dist = strtod(parserbuf, &end);
+			*state = SF0X_PARSE_STATE1_SYNC;
+			*parserbuf_index = 0;
+			ret = 0;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+	}
+
+#ifdef SF0X_DEBUG
+	printf("state: SF0X_PARSE_STATE%s\n", parser_state[*state]);
+#endif
+
+	return ret;
+}

src/drivers/sf0x/sf0x_parser.h

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 sf0x_parser.cpp
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Declarations of parser for the Lightware SF0x laser rangefinder series
+ */
+
+enum SF0X_PARSE_STATE {
+	SF0X_PARSE_STATE0_UNSYNC = 0,
+	SF0X_PARSE_STATE1_SYNC,
+	SF0X_PARSE_STATE2_GOT_DIGIT0,
+	SF0X_PARSE_STATE3_GOT_DOT,
+	SF0X_PARSE_STATE4_GOT_DIGIT1,
+	SF0X_PARSE_STATE5_GOT_DIGIT2,
+	SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN
+};
+
+int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist);