diff options
Diffstat (limited to 'src/drivers')
| -rw-r--r-- | src/drivers/gps/ubx.cpp | 12 | ||||
| -rw-r--r-- | src/drivers/gps/ubx.h | 17 | ||||
| -rw-r--r-- | src/drivers/hil/hil.cpp | 3 | ||||
| -rw-r--r-- | src/drivers/sf0x/module.mk | 3 | ||||
| -rw-r--r-- | src/drivers/sf0x/sf0x.cpp | 100 | ||||
| -rw-r--r-- | src/drivers/sf0x/sf0x_parser.cpp | 155 | ||||
| -rw-r--r-- | src/drivers/sf0x/sf0x_parser.h | 51 |
7 files changed, 255 insertions, 86 deletions
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index d0854f5e9..b0eb4ab66 100644 --- a/src/drivers/gps/ubx.cpp +++ b/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 */ diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 219a5762a..c74eb9168 100644 --- a/src/drivers/gps/ubx.h +++ b/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; diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index f17e99e9d..f0dc0c651 100644 --- a/src/drivers/hil/hil.cpp +++ b/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) { diff --git a/src/drivers/sf0x/module.mk b/src/drivers/sf0x/module.mk index dc2c66d56..26e77d1cc 100644 --- a/src/drivers/sf0x/module.mk +++ b/src/drivers/sf0x/module.mk @@ -37,6 +37,7 @@ MODULE_COMMAND = sf0x -SRCS = sf0x.cpp +SRCS = sf0x.cpp \ + sf0x_parser.cpp MAXOPTIMIZATION = -Os diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index d382d08d0..801bcf40a 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/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; - } - } - - 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; - } + 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; } } - 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; + return -EAGAIN; } + debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); + struct range_finder_report report; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ diff --git a/src/drivers/sf0x/sf0x_parser.cpp b/src/drivers/sf0x/sf0x_parser.cpp new file mode 100644 index 000000000..8e73b0ad3 --- /dev/null +++ b/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; +}
\ No newline at end of file diff --git a/src/drivers/sf0x/sf0x_parser.h b/src/drivers/sf0x/sf0x_parser.h new file mode 100644 index 000000000..20892d50e --- /dev/null +++ b/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);
\ No newline at end of file |