From efdfa2b8b4aa5ef6fb81a237cf8a79276bb4e382 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 14 Nov 2012 00:33:26 +0100 Subject: Setup skeleton code (basic daemon). --- nuttx/configs/px4fmu/nsh/appconfig | 1 + 1 file changed, 1 insertion(+) (limited to 'nuttx') diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 798f57e93..565996083 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -80,6 +80,7 @@ CONFIGURED_APPS += fixedwing_att_control CONFIGURED_APPS += fixedwing_pos_control CONFIGURED_APPS += position_estimator CONFIGURED_APPS += attitude_estimator_ekf +CONFIGURED_APPS += hott_telemetry # Hacking tools #CONFIGURED_APPS += system/i2c -- cgit v1.2.3 From 097aeddcadc6c0b5decf8374c05586bfff5e403d Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 6 Dec 2012 17:24:27 +0100 Subject: Push single wire operations into stm32_serial.c and added a test to verify HoTT telemetry setup. --- apps/px4/tests/test_hott_telemetry.c | 239 ++++++++++++++++++++++++++++++++ apps/px4/tests/tests.h | 1 + apps/px4/tests/tests_main.c | 1 + nuttx/arch/arm/src/stm32/stm32_serial.c | 21 +++ nuttx/drivers/serial/serial.c | 1 + 5 files changed, 263 insertions(+) create mode 100644 apps/px4/tests/test_hott_telemetry.c (limited to 'nuttx') diff --git a/apps/px4/tests/test_hott_telemetry.c b/apps/px4/tests/test_hott_telemetry.c new file mode 100644 index 000000000..dbef021e3 --- /dev/null +++ b/apps/px4/tests/test_hott_telemetry.c @@ -0,0 +1,239 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Simon Wilks + * + * 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_hott_telemetry.c + * + * Tests the Graupner HoTT telemetry support. + * + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "tests.h" + + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +/**************************************************************************** + * Public Data + ****************************************************************************/ + +/**************************************************************************** + * Private Functions + ****************************************************************************/ +static int open_uart(const char *device) +{ + /* baud rate */ + int speed = B19200; + + /* open uart */ + int uart = open(device, O_RDWR | O_NOCTTY); + + if (uart < 0) { + errx(1, "FAIL: Error opening port"); + return ERROR; + } + + /* Try to set baud rate */ + struct termios uart_config; + + /* Fill the struct for the new configuration */ + tcgetattr(uart, &uart_config); + + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* Set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + errx(1, "FAIL: Error setting baudrate / termios config for cfsetispeed, cfsetospeed"); + return ERROR; + } + + if (tcsetattr(uart, TCSANOW, &uart_config) < 0) { + errx(1, "FAIL: Error setting baudrate / termios config for tcsetattr"); + return ERROR; + } + + return uart; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: test_hott_telemetry + ****************************************************************************/ + +int test_hott_telemetry(int argc, char *argv[]) +{ + warnx("HoTT Telemetry Test Requirements:"); + warnx("- Radio on and Electric Air. Mod on (telemetry -> sensor select)."); + warnx("- Receiver telemetry port must be in telemetry mode."); + warnx("- Connect telemetry wire to /dev/ttyS1 (USART2)."); + warnx("Testing..."); + + const char device[] = "/dev/ttyS1"; + int fd = open_uart(device); + + if (fd < 0) { + close(fd); + return ERROR; + } + + /* Activate single wire mode */ + ioctl(fd, TIOCSRS485, SER_RS485_ENABLED); + + char send = 'a'; + write(fd, &send, 1); + + /* Since TX and RX are now connected we should be able to read in what we wrote */ + const int timeout = 1000; + struct pollfd fds[] = { { .fd = fd, .events = POLLIN } }; + + if (poll(fds, 1, timeout) == 0) { + errx(1, "FAIL: Could not read sent data."); + } + + char receive; + read(fd, &receive, 1); + warnx("PASS: Single wire enabled. Sent %x and received %x", send, receive); + + + /* Attempt to read HoTT poll messages from the HoTT receiver */ + int received_count = 0; + int valid_count = 0; + const int max_polls = 5; + uint8_t byte; + + for (; received_count < 5; received_count++) { + if (poll(fds, 1, timeout) == 0) { + errx(1, "FAIL: Could not read sent data. Is your HoTT receiver plugged in on %s?", device); + break; + + } else { + read(fd, &byte, 1); + + if (byte == 0x80) { + valid_count++; + } + + /* Read the device ID being polled */ + read(fd, &byte, 1); + } + } + + if (received_count > 0 && valid_count > 0) { + if (received_count == max_polls && valid_count == max_polls) { + warnx("PASS: Received %d out of %d valid byte pairs from the HoTT receiver device.", received_count, max_polls); + + } else { + warnx("WARN: Received %d out of %d byte pairs of which %d were valid from the HoTT receiver device.", received_count, max_polls, valid_count); + } + + } else { + /* Let's work out what went wrong */ + if (received_count == 0) { + errx(1, "FAIL: Could not read any polls from HoTT receiver device."); + } + + if (valid_count == 0) { + errx(1, "FAIL: Received unexpected values from the HoTT receiver device."); + } + } + + + /* Attempt to send a HoTT response messages */ + uint8_t response[] = {0x7c, 0x8e, 0x00, 0xe0, 0x00, 0x80, 0x00, 0x00, 0x00, 0x00, \ + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, \ + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xf4, 0x01, 0x00, 0x00, \ + 0x19, 0x00, 0x00, 0x00, 0x30, 0x75, 0x78, 0x00, 0x00, 0x00, \ + 0x00, 0x00, 0x00, 0x7d, 0x12 + }; + + usleep(5000); + + for (unsigned int i = 0; i < sizeof(response); i++) { + write(fd, &response[i], 1); + usleep(1000); + } + + warnx("PASS: Response sent to the HoTT receiver device. Voltage should now show 2.5V."); + + + /* Disable single wire */ + ioctl(fd, TIOCSRS485, ~SER_RS485_ENABLED); + + write(fd, &send, 1); + + /* We should timeout as there will be nothing to read (TX and RX no longer connected) */ + if (poll(fds, 1, timeout) == 0) { + errx(1, "FAIL: timeout expected."); + } + + warnx("PASS: Single wire disabled."); + + close(fd); + exit(0); +} diff --git a/apps/px4/tests/tests.h b/apps/px4/tests/tests.h index 46450d10b..cc3f5493a 100644 --- a/apps/px4/tests/tests.h +++ b/apps/px4/tests/tests.h @@ -93,6 +93,7 @@ extern int test_uart_send(int argc, char *argv[]); extern int test_sleep(int argc, char *argv[]); extern int test_time(int argc, char *argv[]); extern int test_uart_console(int argc, char *argv[]); +extern int test_hott_telemetry(int argc, char *argv[]); 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[]); diff --git a/apps/px4/tests/tests_main.c b/apps/px4/tests/tests_main.c index 26f7ef96b..906d25f9e 100644 --- a/apps/px4/tests/tests_main.c +++ b/apps/px4/tests/tests_main.c @@ -101,6 +101,7 @@ struct { {"uart_baudchange", test_uart_baudchange, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, {"uart_send", test_uart_send, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, {"uart_console", test_uart_console, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, + {"hott_telemetry", test_hott_telemetry, OPT_NOJIGTEST | OPT_NOALLTEST, 0}, {"tone", test_tone, 0, 0}, {"sleep", test_sleep, OPT_NOJIGTEST, 0}, {"time", test_time, OPT_NOJIGTEST, 0}, diff --git a/nuttx/arch/arm/src/stm32/stm32_serial.c b/nuttx/arch/arm/src/stm32/stm32_serial.c index 0868c3cd3..80c4ce357 100644 --- a/nuttx/arch/arm/src/stm32/stm32_serial.c +++ b/nuttx/arch/arm/src/stm32/stm32_serial.c @@ -1307,6 +1307,27 @@ static int up_ioctl(struct file *filep, int cmd, unsigned long arg) } break; + /* Set RS485 mode */ + case TIOCSRS485: + { + /* Change the TX port to be open-drain/push-pull */ + if (arg == SER_RS485_ENABLED) { + stm32_configgpio(priv->tx_gpio | GPIO_OPENDRAIN); + } else { + stm32_configgpio(priv->tx_gpio | GPIO_PUSHPULL); + } + + /* Enable/disable half-duplex mode */ + uint32_t cr = up_serialin(priv, STM32_USART_CR3_OFFSET); + if (arg == SER_RS485_ENABLED) { + cr |= (USART_CR3_HDSEL); + } else { + cr &= ~(USART_CR3_HDSEL); + } + up_serialout(priv, STM32_USART_CR3_OFFSET, cr); + } + break; + #ifdef CONFIG_SERIAL_TERMIOS case TCGETS: { diff --git a/nuttx/drivers/serial/serial.c b/nuttx/drivers/serial/serial.c index c650da5db..c31d12eee 100644 --- a/nuttx/drivers/serial/serial.c +++ b/nuttx/drivers/serial/serial.c @@ -687,6 +687,7 @@ static int uart_ioctl(FAR struct file *filep, int cmd, unsigned long arg) *(int *)arg = count; } + break; #ifdef CONFIG_SERIAL_TERMIOS case TCGETS: -- cgit v1.2.3 From ecd01dc2e88a60d71ad87968cd07234a32bf7d8d Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Mon, 21 Jan 2013 23:31:12 +0100 Subject: We aren't using RS485 but single wire. --- apps/px4/tests/test_hott_telemetry.c | 4 ++-- nuttx/arch/arm/src/stm32/stm32_serial.c | 8 ++++---- nuttx/include/nuttx/serial/tioctl.h | 6 ++++++ 3 files changed, 12 insertions(+), 6 deletions(-) (limited to 'nuttx') diff --git a/apps/px4/tests/test_hott_telemetry.c b/apps/px4/tests/test_hott_telemetry.c index dbef021e3..74aa0e614 100644 --- a/apps/px4/tests/test_hott_telemetry.c +++ b/apps/px4/tests/test_hott_telemetry.c @@ -143,7 +143,7 @@ int test_hott_telemetry(int argc, char *argv[]) } /* Activate single wire mode */ - ioctl(fd, TIOCSRS485, SER_RS485_ENABLED); + ioctl(fd, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); char send = 'a'; write(fd, &send, 1); @@ -223,7 +223,7 @@ int test_hott_telemetry(int argc, char *argv[]) /* Disable single wire */ - ioctl(fd, TIOCSRS485, ~SER_RS485_ENABLED); + ioctl(fd, TIOCSSINGLEWIRE, ~SER_SINGLEWIRE_ENABLED); write(fd, &send, 1); diff --git a/nuttx/arch/arm/src/stm32/stm32_serial.c b/nuttx/arch/arm/src/stm32/stm32_serial.c index 80c4ce357..7e8537b2a 100644 --- a/nuttx/arch/arm/src/stm32/stm32_serial.c +++ b/nuttx/arch/arm/src/stm32/stm32_serial.c @@ -1307,11 +1307,11 @@ static int up_ioctl(struct file *filep, int cmd, unsigned long arg) } break; - /* Set RS485 mode */ - case TIOCSRS485: + /* Set single-wire mode */ + case TIOCSSINGLEWIRE: { /* Change the TX port to be open-drain/push-pull */ - if (arg == SER_RS485_ENABLED) { + if (arg == SER_SINGLEWIRE_ENABLED) { stm32_configgpio(priv->tx_gpio | GPIO_OPENDRAIN); } else { stm32_configgpio(priv->tx_gpio | GPIO_PUSHPULL); @@ -1319,7 +1319,7 @@ static int up_ioctl(struct file *filep, int cmd, unsigned long arg) /* Enable/disable half-duplex mode */ uint32_t cr = up_serialin(priv, STM32_USART_CR3_OFFSET); - if (arg == SER_RS485_ENABLED) { + if (arg == SER_SINGLEWIRE_ENABLED) { cr |= (USART_CR3_HDSEL); } else { cr &= ~(USART_CR3_HDSEL); diff --git a/nuttx/include/nuttx/serial/tioctl.h b/nuttx/include/nuttx/serial/tioctl.h index b309ff37c..76185a22e 100644 --- a/nuttx/include/nuttx/serial/tioctl.h +++ b/nuttx/include/nuttx/serial/tioctl.h @@ -169,6 +169,9 @@ #define TIOCSERGSTRUCT _TIOC(0x002c) /* Get device TTY structure */ +#define TIOCSSINGLEWIRE _TIOC(0x002d) /* Set single-wire mode */ +#define TIOCGSINGLEWIRE _TIOC(0x002e) /* Get single-wire mode */ + /* Definitions used in struct serial_rs485 (Linux compatible) */ #define SER_RS485_ENABLED (1 << 0) /* Enable/disble RS-485 support */ @@ -176,6 +179,9 @@ #define SER_RS485_RTS_AFTER_SEND (1 << 2) /* Logic level for RTS pin after sent */ #define SER_RS485_RX_DURING_TX (1 << 4) +/* single-wire definitions */ +#define SER_SINGLEWIRE_ENABLED (1 << 0) /* Enable/disble single-wire support */ + /******************************************************************************************** * Public Type Definitions ********************************************************************************************/ -- cgit v1.2.3 From 0246842c8854fcb49ffa34bcf46c61f2b7da95b2 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 25 Jan 2013 01:41:11 +0100 Subject: Enable single wire via ioctl calls and pull the battery voltage from the battery status topic. --- apps/hott_telemetry/hott_telemetry_main.c | 37 ++----------------------------- apps/hott_telemetry/messages.c | 10 ++++++++- nuttx/include/nuttx/serial/tioctl.h | 2 +- 3 files changed, 12 insertions(+), 37 deletions(-) (limited to 'nuttx') diff --git a/apps/hott_telemetry/hott_telemetry_main.c b/apps/hott_telemetry/hott_telemetry_main.c index ae6c69b99..d67ab06a9 100644 --- a/apps/hott_telemetry/hott_telemetry_main.c +++ b/apps/hott_telemetry/hott_telemetry_main.c @@ -134,41 +134,8 @@ static int open_uart(const char *device, struct termios *uart_config_original) FATAL_MSG(msg); } - /* Get the appropriate GPIO pin and control register */ - uint32_t gpio_uart = GPIO_USART2_TX;; - uint32_t uart_cr3 = STM32_USART2_CR3; - - switch (device[strlen(device) - 1]) { - case '0': - gpio_uart = GPIO_USART1_TX; - uart_cr3 = STM32_USART1_CR3; - break; - - case '1': - gpio_uart = GPIO_USART2_TX; - uart_cr3 = STM32_USART2_CR3; - break; - - case '3': - gpio_uart = GPIO_USART6_TX; - uart_cr3 = STM32_USART6_CR3; - break; - - default: - sprintf(msg, "%s is not supported.\n", device); - close(uart); - FATAL_MSG(msg); - break; - } - - /* Change the TX port to be open-drain */ - stm32_configgpio(gpio_uart | GPIO_OPENDRAIN); - - /* Turn on half-duplex mode */ - uint32_t cr; - cr = getreg32(uart_cr3); - cr |= (USART_CR3_HDSEL); - putreg32(cr, uart_cr3); + /* Activate single wire mode */ + ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); return uart; } diff --git a/apps/hott_telemetry/messages.c b/apps/hott_telemetry/messages.c index dce16f371..8bfb99773 100644 --- a/apps/hott_telemetry/messages.c +++ b/apps/hott_telemetry/messages.c @@ -42,12 +42,15 @@ #include #include #include +#include #include +static int battery_sub = -1; static int sensor_sub = -1; void messages_init(void) { + battery_sub = orb_subscribe(ORB_ID(battery_status)); sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); } @@ -58,6 +61,11 @@ void build_eam_response(uint8_t *buffer, int *size) memset(&raw, 0, sizeof(raw)); orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + /* get a local copy of the battery data */ + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + struct eam_module_msg msg; *size = sizeof(msg); memset(&msg, 0, *size); @@ -67,7 +75,7 @@ void build_eam_response(uint8_t *buffer, int *size) msg.sensor_id = EAM_SENSOR_ID; msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20); msg.temperature2 = TEMP_ZERO_CELSIUS; - msg.main_voltage_L = (uint8_t)(raw.battery_voltage_v * 10); + msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10); uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500); msg.altitude_L = (uint8_t)alt & 0xff; diff --git a/nuttx/include/nuttx/serial/tioctl.h b/nuttx/include/nuttx/serial/tioctl.h index 9edc7e87f..a98b487a6 100644 --- a/nuttx/include/nuttx/serial/tioctl.h +++ b/nuttx/include/nuttx/serial/tioctl.h @@ -177,7 +177,7 @@ #define TIOCSSINGLEWIRE _TIOC(0x002c) /* Set single-wire mode */ #define TIOCGSINGLEWIRE _TIOC(0x002d) /* Get single-wire mode */ -# define SER_SINGLEWIRE_ENABLED (1 << 0) /* Enable/disble single-wire support */ +# define SER_SINGLEWIRE_ENABLED (1 << 0) /* Enable/disable single-wire support */ /* Debugging */ -- cgit v1.2.3 From bd5887b4cae08dd378c03280e6c3ddafcec729c1 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 25 Jan 2013 10:44:21 +0100 Subject: Move the config param to a more sane location (I hope). --- nuttx/configs/px4fmu/nsh/defconfig | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) (limited to 'nuttx') diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index d669da83a..fd783dec5 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -200,6 +200,12 @@ CONFIG_STM32_TIM9=y CONFIG_STM32_TIM10=y CONFIG_STM32_TIM11=y +# +# Enable single wire support. If this is not defined, then this mode cannot +# be enabled. +# +CONFIG_STM32_USART_SINGLEWIRE=y + # # We want the flash prefetch on for max performance. # @@ -226,8 +232,6 @@ CONFIG_AT24XX_MTD_BLOCKSIZE=256 # CONFIG_SERIAL_CONSOLE_REINIT - re-initializes the console serial port # immediately after creating the /dev/console device. This is required # if the console serial port has RX DMA enabled. -# CONFIG_STM32_USART_SINGLEWIRE - Serial driver supports single wire mode. If -# this is not defined, then this mode cannot be enabled. # # CONFIG_USARTn_SERIAL_CONSOLE - selects the USARTn for the # console and ttys0 (default is the USART1). @@ -242,7 +246,6 @@ CONFIG_AT24XX_MTD_BLOCKSIZE=256 # CONFIG_SERIAL_TERMIOS=y CONFIG_SERIAL_CONSOLE_REINIT=y -CONFIG_STM32_USART_SINGLEWIRE=y CONFIG_USART1_SERIAL_CONSOLE=y CONFIG_USART2_SERIAL_CONSOLE=n -- cgit v1.2.3