diff options
Diffstat (limited to 'src/drivers')
-rw-r--r-- | src/drivers/boards/px4fmu-v1/board_config.h | 3 | ||||
-rw-r--r-- | src/drivers/boards/px4fmu-v2/board_config.h | 7 | ||||
-rw-r--r-- | src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 12 | ||||
-rw-r--r-- | src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 35 | ||||
-rw-r--r-- | src/drivers/drv_io_expander.h | 71 | ||||
-rw-r--r-- | src/drivers/gps/ubx.cpp | 18 | ||||
-rw-r--r-- | src/drivers/gps/ubx.h | 3 | ||||
-rw-r--r-- | src/drivers/lsm303d/lsm303d.cpp | 2 | ||||
-rw-r--r-- | src/drivers/pca8574/module.mk | 6 | ||||
-rw-r--r-- | src/drivers/pca8574/pca8574.cpp | 554 | ||||
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 38 | ||||
-rw-r--r-- | src/drivers/px4io/px4io_uploader.cpp | 6 |
12 files changed, 729 insertions, 26 deletions
diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 58273f2d2..c944007a5 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -86,6 +86,7 @@ __BEGIN_DECLS #define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) #define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_EXT 2 /* * Use these in place of the spi_dev_e enumeration to @@ -98,7 +99,7 @@ __BEGIN_DECLS /* * Optional devices on IO's external port */ -#define PX4_SPIDEV_ACCEL_MAG 2 +#define PX4_SPIDEV_ACCEL_MAG 2 /* * I2C busses diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index c2de1bfba..36eb7bec4 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -106,8 +106,11 @@ __BEGIN_DECLS #define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) #define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) +#define GPIO_SPI_CS_EXT0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) +#define GPIO_SPI_CS_EXT1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) #define PX4_SPI_BUS_SENSORS 1 +#define PX4_SPI_BUS_EXT 4 /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO 1 @@ -115,6 +118,10 @@ __BEGIN_DECLS #define PX4_SPIDEV_BARO 3 #define PX4_SPIDEV_MPU 4 +/* External bus */ +#define PX4_SPIDEV_EXT0 1 +#define PX4_SPIDEV_EXT1 2 + /* I2C busses */ #define PX4_I2C_BUS_EXPANSION 1 #define PX4_I2C_BUS_LED 2 diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 71414d62c..bf41bb1fe 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -192,6 +192,7 @@ stm32_boardinitialize(void) static struct spi_dev_s *spi1; static struct spi_dev_s *spi2; +static struct spi_dev_s *spi4; static struct sdio_dev_s *sdio; #include <math.h> @@ -305,6 +306,17 @@ __EXPORT int nsh_archinitialize(void) message("[boot] Initialized SPI port 2 (RAMTRON FRAM)\n"); + spi4 = up_spiinitialize(4); + + /* Default SPI4 to 1MHz and de-assert the known chip selects. */ + SPI_SETFREQUENCY(spi4, 10000000); + SPI_SETBITS(spi4, 8); + SPI_SETMODE(spi4, SPIDEV_MODE3); + SPI_SELECT(spi4, PX4_SPIDEV_EXT0, false); + SPI_SELECT(spi4, PX4_SPIDEV_EXT1, false); + + message("[boot] Initialized SPI port 4\n"); + #ifdef CONFIG_MMCSD /* First, get an instance of the SDIO interface */ diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index c66c490a7..01dbd6e77 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -94,6 +94,13 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_configgpio(GPIO_SPI_CS_FRAM); stm32_gpiowrite(GPIO_SPI_CS_FRAM, 1); #endif + +#ifdef CONFIG_STM32_SPI4 + stm32_configgpio(GPIO_SPI_CS_EXT0); + stm32_configgpio(GPIO_SPI_CS_EXT1); + stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); +#endif } __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) @@ -157,3 +164,31 @@ __EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devi return SPI_STATUS_PRESENT; } #endif + +__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_EXT0: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_EXT0, !selected); + stm32_gpiowrite(GPIO_SPI_CS_EXT1, 1); + break; + + case PX4_SPIDEV_EXT1: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_EXT1, !selected); + stm32_gpiowrite(GPIO_SPI_CS_EXT0, 1); + break; + + default: + break; + + } +} + +__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} diff --git a/src/drivers/drv_io_expander.h b/src/drivers/drv_io_expander.h new file mode 100644 index 000000000..106354377 --- /dev/null +++ b/src/drivers/drv_io_expander.h @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * 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 drv_io_expander.h + * + * IO expander device API + */ + +#pragma once + +#include <stdint.h> +#include <sys/ioctl.h> + +/* + * ioctl() definitions + */ + +#define _IOXIOCBASE (0x2800) +#define _IOXIOC(_n) (_IOC(_IOXIOCBASE, _n)) + +/** set a bitmask (non-blocking) */ +#define IOX_SET_MASK _IOXIOC(1) + +/** get a bitmask (blocking) */ +#define IOX_GET_MASK _IOXIOC(2) + +/** set device mode (non-blocking) */ +#define IOX_SET_MODE _IOXIOC(3) + +/** set constant values (non-blocking) */ +#define IOX_SET_VALUE _IOXIOC(4) + +/* ... to IOX_SET_VALUE + 8 */ + +/* enum passed to RGBLED_SET_MODE ioctl()*/ +enum IOX_MODE { + IOX_MODE_OFF, + IOX_MODE_ON, + IOX_MODE_TEST_OUT +}; diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 19cf5beec..c143eeb0c 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -69,6 +69,9 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : _gps_position(gps_position), _configured(false), _waiting_for_ack(false), + _got_posllh(false), + _got_velned(false), + _got_timeutc(false), _disable_cmd_last(0) { decode_init(); @@ -275,9 +278,10 @@ UBX::receive(unsigned timeout) bool handled = false; while (true) { + bool ready_to_return = _configured ? (_got_posllh && _got_velned && _got_timeutc) : handled; /* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */ - int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), handled ? UBX_PACKET_TIMEOUT : timeout); + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout); if (ret < 0) { /* something went wrong when polling */ @@ -286,7 +290,10 @@ UBX::receive(unsigned timeout) } else if (ret == 0) { /* return success after short delay after receiving a packet or timeout after long delay */ - if (handled) { + if (ready_to_return) { + _got_posllh = false; + _got_velned = false; + _got_timeutc = false; return 1; } else { @@ -438,6 +445,7 @@ UBX::handle_message() _rate_count_lat_lon++; + _got_posllh = true; ret = 1; break; } @@ -447,8 +455,8 @@ UBX::handle_message() gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; _gps_position->fix_type = packet->gpsFix; - _gps_position->s_variance_m_s = packet->sAcc; - _gps_position->p_variance_m = packet->pAcc; + _gps_position->s_variance_m_s = (float)packet->sAcc * 1e-2f; // from cm/s to m/s + _gps_position->p_variance_m = (float)packet->pAcc * 1e-2f; // from cm to m _gps_position->timestamp_variance = hrt_absolute_time(); ret = 1; @@ -482,6 +490,7 @@ UBX::handle_message() _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); _gps_position->timestamp_time = hrt_absolute_time(); + _got_timeutc = true; ret = 1; break; } @@ -557,6 +566,7 @@ UBX::handle_message() _rate_count_vel++; + _got_velned = true; ret = 1; break; } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 5cf47b60b..43d688893 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -397,6 +397,9 @@ private: struct vehicle_gps_position_s *_gps_position; bool _configured; bool _waiting_for_ack; + bool _got_posllh; + bool _got_velned; + bool _got_timeutc; uint8_t _message_class_needed; uint8_t _message_id_needed; ubx_decode_state_t _decode_state; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 4ca8b5e42..8bf76dcc3 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -880,7 +880,7 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen) /* manual measurement */ _mag_reports->flush(); - measure(); + _mag->measure(); /* measurement will have generated a report, copy it out */ if (_mag_reports->get(mrb)) diff --git a/src/drivers/pca8574/module.mk b/src/drivers/pca8574/module.mk new file mode 100644 index 000000000..825ee9bb7 --- /dev/null +++ b/src/drivers/pca8574/module.mk @@ -0,0 +1,6 @@ +# +# PCA8574 driver for RGB LED +# + +MODULE_COMMAND = pca8574 +SRCS = pca8574.cpp diff --git a/src/drivers/pca8574/pca8574.cpp b/src/drivers/pca8574/pca8574.cpp new file mode 100644 index 000000000..904ce18e8 --- /dev/null +++ b/src/drivers/pca8574/pca8574.cpp @@ -0,0 +1,554 @@ +/**************************************************************************** + * + * Copyright (c) 2012-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 pca8574.cpp + * + * Driver for an 8 I/O controller (PC8574) connected via I2C. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include <nuttx/config.h> + +#include <drivers/device/i2c.h> + +#include <sys/types.h> +#include <stdint.h> +#include <string.h> +#include <stdlib.h> +#include <stdbool.h> +#include <fcntl.h> +#include <unistd.h> +#include <stdio.h> +#include <ctype.h> + +#include <nuttx/wqueue.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> + +#include <board_config.h> + +#include <drivers/drv_io_expander.h> + +#define PCA8574_ONTIME 120 +#define PCA8574_OFFTIME 120 +#define PCA8574_DEVICE_PATH "/dev/pca8574" + +#define ADDR 0x20 ///< I2C adress of PCA8574 (default, A0-A2 pulled to GND) + +class PCA8574 : public device::I2C +{ +public: + PCA8574(int bus, int pca8574); + virtual ~PCA8574(); + + + virtual int init(); + virtual int probe(); + virtual int info(); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + bool is_running() { return _running; } + +private: + work_s _work; + + uint8_t _values_out; + uint8_t _values_in; + uint8_t _blinking; + uint8_t _blink_phase; + + enum IOX_MODE _mode; + bool _running; + int _led_interval; + bool _should_run; + bool _update_out; + int _counter; + + static void led_trampoline(void *arg); + void led(); + + int send_led_enable(uint8_t arg); + int send_led_values(); + + int get(uint8_t &vals); +}; + +/* for now, we only support one PCA8574 */ +namespace +{ +PCA8574 *g_pca8574; +} + +void pca8574_usage(); + +extern "C" __EXPORT int pca8574_main(int argc, char *argv[]); + +PCA8574::PCA8574(int bus, int pca8574) : + I2C("pca8574", PCA8574_DEVICE_PATH, bus, pca8574, 100000), + _values_out(0), + _values_in(0), + _blinking(0), + _blink_phase(0), + _mode(IOX_MODE_OFF), + _running(false), + _led_interval(80), + _should_run(false), + _update_out(false), + _counter(0) +{ + memset(&_work, 0, sizeof(_work)); +} + +PCA8574::~PCA8574() +{ +} + +int +PCA8574::init() +{ + int ret; + ret = I2C::init(); + + if (ret != OK) { + return ret; + } + + return OK; +} + +int +PCA8574::probe() +{ + uint8_t val; + return get(val); +} + +int +PCA8574::info() +{ + int ret = OK; + + return ret; +} + +int +PCA8574::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = ENOTTY; + + switch (cmd) { + case IOX_SET_VALUE ...(IOX_SET_VALUE + 8): { + // set the specified on / off state + uint8_t position = (1 << (cmd - IOX_SET_VALUE)); + uint8_t prev = _values_out; + + if (arg) { + _values_out |= position; + + } else { + _values_out &= ~(position); + } + + if (_values_out != prev) { + if (_values_out) { + _mode = IOX_MODE_ON; + } + send_led_values(); + } + + return OK; + } + + case IOX_SET_MASK: + send_led_enable(arg); + return OK; + + case IOX_GET_MASK: { + uint8_t val; + ret = get(val); + + if (ret == OK) { + return val; + + } else { + return -1; + } + } + + case IOX_SET_MODE: + + if (_mode != (IOX_MODE)arg) { + + switch ((IOX_MODE)arg) { + case IOX_MODE_OFF: + _values_out = 0xFF; + break; + + case IOX_MODE_ON: + _values_out = 0; + break; + + case IOX_MODE_TEST_OUT: + break; + + default: + return -1; + } + + _mode = (IOX_MODE)arg; + send_led_values(); + } + + return OK; + + default: + // see if the parent class can make any use of it + ret = CDev::ioctl(filp, cmd, arg); + break; + } + + return ret; +} + + +void +PCA8574::led_trampoline(void *arg) +{ + PCA8574 *rgbl = reinterpret_cast<PCA8574 *>(arg); + + rgbl->led(); +} + +/** + * Main loop function + */ +void +PCA8574::led() +{ + if (_mode == IOX_MODE_TEST_OUT) { + + // we count only seven states + _counter &= 0xF; + _counter++; + + for (int i = 0; i < 8; i++) { + if (i < _counter) { + _values_out |= (1 << i); + + } else { + _values_out &= ~(1 << i); + } + } + + _update_out = true; + _should_run = true; + } else if (_mode == IOX_MODE_OFF) { + _update_out = true; + _should_run = false; + } else { + + // Any of the normal modes + if (_blinking > 0) { + /* we need to be running to blink */ + _should_run = true; + } else { + _should_run = false; + } + } + + if (_update_out) { + uint8_t msg; + + if (_blinking) { + msg = (_values_out & _blinking & _blink_phase); + + // wipe out all positions that are marked as blinking + msg &= ~(_blinking); + + // fill blink positions + msg |= ((_blink_phase) ? _blinking : 0); + + _blink_phase = !_blink_phase; + } else { + msg = _values_out; + } + + int ret = transfer(&msg, sizeof(msg), nullptr, 0); + + if (!ret) { + _update_out = false; + } + } + + // check if any activity remains, else stp + if (!_should_run) { + _running = false; + return; + } + + // re-queue ourselves to run again later + _running = true; + work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, _led_interval); +} + +/** + * Sent ENABLE flag to LED driver + */ +int +PCA8574::send_led_enable(uint8_t arg) +{ + + int ret = transfer(&arg, sizeof(arg), nullptr, 0); + + return ret; +} + +/** + * Send 8 outputs + */ +int +PCA8574::send_led_values() +{ + _update_out = true; + + // if not active, kick it + if (!_running) { + _running = true; + work_queue(LPWORK, &_work, (worker_t)&PCA8574::led_trampoline, this, 1); + } + + return 0; +} + +int +PCA8574::get(uint8_t &vals) +{ + uint8_t result; + int ret; + + ret = transfer(nullptr, 0, &result, 1); + + if (ret == OK) { + _values_in = result; + vals = result; + } + + return ret; +} + +void +pca8574_usage() +{ + warnx("missing command: try 'start', 'test', 'info', 'off', 'stop', 'val 0 1'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); + warnx(" -a addr (0x%x)", ADDR); +} + +int +pca8574_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int pca8574adr = ADDR; // 7bit + + int ch; + + // jump over start/off/etc and look at options first + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + pca8574adr = strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = strtol(optarg, NULL, 0); + break; + + default: + pca8574_usage(); + exit(0); + } + } + + if (optind >= argc) { + pca8574_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + int fd; + int ret; + + if (!strcmp(verb, "start")) { + if (g_pca8574 != nullptr) { + errx(1, "already started"); + } + + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_pca8574 = new PCA8574(PX4_I2C_BUS_EXPANSION, pca8574adr); + + if (g_pca8574 != nullptr && OK != g_pca8574->init()) { + delete g_pca8574; + g_pca8574 = nullptr; + } + + if (g_pca8574 == nullptr) { + // fall back to default bus + if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) { + errx(1, "init failed"); + } + + i2cdevice = PX4_I2C_BUS_LED; + } + } + + if (g_pca8574 == nullptr) { + g_pca8574 = new PCA8574(i2cdevice, pca8574adr); + + if (g_pca8574 == nullptr) { + errx(1, "new failed"); + } + + if (OK != g_pca8574->init()) { + delete g_pca8574; + g_pca8574 = nullptr; + errx(1, "init failed"); + } + } + + exit(0); + } + + // need the driver past this point + if (g_pca8574 == nullptr) { + warnx("not started, run pca8574 start"); + exit(1); + } + + if (!strcmp(verb, "test")) { + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT); + + close(fd); + exit(ret); + } + + if (!strcmp(verb, "info")) { + g_pca8574->info(); + exit(0); + } + + if (!strcmp(verb, "off")) { + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd < 0) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); + close(fd); + exit(ret); + } + + if (!strcmp(verb, "stop")) { + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); + close(fd); + + // wait until we're not running any more + for (unsigned i = 0; i < 15; i++) { + if (!g_pca8574->is_running()) { + break; + } + + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + fflush(stdout); + + if (!g_pca8574->is_running()) { + delete g_pca8574; + g_pca8574 = nullptr; + exit(0); + } else { + warnx("stop failed."); + exit(1); + } + } + + if (!strcmp(verb, "val")) { + if (argc < 4) { + errx(1, "Usage: pca8574 val <channel> <0 or 1>"); + } + + fd = open(PCA8574_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA8574_DEVICE_PATH); + } + + unsigned channel = strtol(argv[2], NULL, 0); + unsigned val = strtol(argv[3], NULL, 0); + + if (channel < 8) { + ret = ioctl(fd, (IOX_SET_VALUE + channel), val); + } else { + ret = -1; + } + close(fd); + exit(ret); + } + + pca8574_usage(); + exit(0); +} diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index c6acfe6be..24da4c68b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -198,8 +198,10 @@ public: * Print IO status. * * Print all relevant IO status information + * + * @param extended_status Shows more verbose information (in particular RC config) */ - void print_status(); + void print_status(bool extended_status); /** * Fetch and print debug console output. @@ -1864,7 +1866,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) } void -PX4IO::print_status() +PX4IO::print_status(bool extended_status) { /* basic configuration */ printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n", @@ -2027,19 +2029,21 @@ PX4IO::print_status() printf("\n"); } - for (unsigned i = 0; i < _max_rc_input; i++) { - unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; - uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); - printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n", - i, - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), - options, - ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), - ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); + if (extended_status) { + for (unsigned i = 0; i < _max_rc_input; i++) { + unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; + uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); + printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n", + i, + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), + options, + ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), + ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); + } } printf("failsafe"); @@ -2867,7 +2871,7 @@ monitor(void) if (g_dev != nullptr) { printf("\033[2J\033[H"); /* move cursor home and clear screen */ - (void)g_dev->print_status(); + (void)g_dev->print_status(false); (void)g_dev->print_debug(); printf("\n\n\n[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n"); @@ -3133,7 +3137,7 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { printf("[px4io] loaded\n"); - g_dev->print_status(); + g_dev->print_status(true); exit(0); } diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 28ec62356..7b6361a7c 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -240,9 +240,9 @@ PX4IO_Uploader::upload(const char *filenames[]) close(_io_fd); _io_fd = -1; - // sleep for enough time for the IO chip to boot. This makes - // forceupdate more reliably startup IO again after update - up_udelay(100*1000); + // sleep for enough time for the IO chip to boot. This makes + // forceupdate more reliably startup IO again after update + up_udelay(100*1000); return ret; } |