From 32498009a36553a1f50601e205ebd609e894a8df Mon Sep 17 00:00:00 2001 From: Nicolae Rosia Date: Sun, 14 May 2017 09:54:15 +0300 Subject: [PATCH] navio_adc: add driver for Navio2 ADC Signed-off-by: Nicolae Rosia --- cmake/configs/posix_rpi_common.cmake | 1 + posix-configs/rpi/px4.config | 1 + posix-configs/rpi/px4_fw.config | 1 + src/drivers/navio_adc/CMakeLists.txt | 42 ++++ src/drivers/navio_adc/navio_adc.cpp | 283 +++++++++++++++++++++++++++ src/modules/sensors/sensors.cpp | 2 +- 6 files changed, 329 insertions(+), 1 deletion(-) create mode 100644 src/drivers/navio_adc/CMakeLists.txt create mode 100644 src/drivers/navio_adc/navio_adc.cpp diff --git a/cmake/configs/posix_rpi_common.cmake b/cmake/configs/posix_rpi_common.cmake index f956f79589..ba0120e05f 100644 --- a/cmake/configs/posix_rpi_common.cmake +++ b/cmake/configs/posix_rpi_common.cmake @@ -72,6 +72,7 @@ set(config_module_list # PX4 drivers # drivers/gps + drivers/navio_adc drivers/navio_sysfs_rc_in drivers/navio_sysfs_pwm_out drivers/navio_gpio diff --git a/posix-configs/rpi/px4.config b/posix-configs/rpi/px4.config index 501f2660ab..f1340fb001 100644 --- a/posix-configs/rpi/px4.config +++ b/posix-configs/rpi/px4.config @@ -10,6 +10,7 @@ df_lsm9ds1_wrapper start -R 4 #df_hmc5883_wrapper start df_ms5611_wrapper start navio_rgbled start +navio_adc start gps start -d /dev/spidev0.0 -i spi -p ubx sensors start commander start diff --git a/posix-configs/rpi/px4_fw.config b/posix-configs/rpi/px4_fw.config index aa32d46f5e..c2c838b7a1 100644 --- a/posix-configs/rpi/px4_fw.config +++ b/posix-configs/rpi/px4_fw.config @@ -8,6 +8,7 @@ dataman start df_lsm9ds1_wrapper start -R 4 df_ms5611_wrapper start navio_rgbled start +navio_adc start gps start -d /dev/spidev0.0 -i spi -p ubx sensors start commander start diff --git a/src/drivers/navio_adc/CMakeLists.txt b/src/drivers/navio_adc/CMakeLists.txt new file mode 100644 index 0000000000..cfe39b33d3 --- /dev/null +++ b/src/drivers/navio_adc/CMakeLists.txt @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2015-2017 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. +# +############################################################################ +px4_add_module( + MODULE drivers__navio_adc + MAIN navio_adc + COMPILE_FLAGS + SRCS + navio_adc.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/navio_adc/navio_adc.cpp b/src/drivers/navio_adc/navio_adc.cpp new file mode 100644 index 0000000000..4ce89997fc --- /dev/null +++ b/src/drivers/navio_adc/navio_adc.cpp @@ -0,0 +1,283 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2017 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 navio_adc.cpp + * + * Navio2 ADC Driver + * + * This driver exports the sysfs-based ADC driver on Navio2. + * + * @author Nicolae Rosia + */ + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#define ADC_BASE_DEV_PATH "/dev/adc" +#define ADC_SYSFS_PATH "/sys/kernel/rcio/adc/ch0" +#define ADC_MAX_CHAN 6 + +/* + * ADC Channels: + * A0 - Board voltage (5V) + * A1 - servo rail voltage + * A2 - power module voltage (ADC0, POWER port) + * A3 - power module current (ADC1, POWER port) + * A4 - ADC2 (ADC port) + * A5 - ADC3 (ADC port) + */ + +#define NAVIO_ADC_BATTERY_VOLTAGE_CHANNEL (2) +#define NAVIO_ADC_BATTERY_CURRENT_CHANNEL (3) + +__BEGIN_DECLS +__EXPORT int navio_adc_main(int argc, char *argv[]); +__END_DECLS + +class NavioADC: public DriverFramework::VirtDevObj +{ +public: + NavioADC(); + virtual ~NavioADC(); + + virtual int init(); + + virtual ssize_t devRead(void *buf, size_t count) override; + virtual int devIOCTL(unsigned long request, unsigned long arg) override; + +protected: + virtual void _measure() override; + +private: + int read_channel(struct adc_msg_s *adc_msg, int channel); + + pthread_mutex_t _samples_lock; + adc_msg_s _samples[ADC_MAX_CHAN]; +}; + +NavioADC::NavioADC() + : DriverFramework::VirtDevObj("navio_adc", ADC0_DEVICE_PATH, ADC_BASE_DEV_PATH, 1e6 / 100) +{ + pthread_mutex_init(&_samples_lock, NULL); +} + +NavioADC::~NavioADC() +{ + pthread_mutex_destroy(&_samples_lock); +} + +void NavioADC::_measure() +{ + adc_msg_s tmp_samples[ADC_MAX_CHAN]; + + for (int i = 0; i < ADC_MAX_CHAN; ++i) { + int ret = read_channel(&tmp_samples[i], i); + if (ret != 0) { + PX4_ERR("read_channel(%d): %d", i, ret); + tmp_samples[i].am_channel = i; + tmp_samples[i].am_data = 0; + } + } + + tmp_samples[NAVIO_ADC_BATTERY_VOLTAGE_CHANNEL].am_channel = ADC_BATTERY_VOLTAGE_CHANNEL; + tmp_samples[NAVIO_ADC_BATTERY_CURRENT_CHANNEL].am_channel = ADC_BATTERY_CURRENT_CHANNEL; + + pthread_mutex_lock(&_samples_lock); + memcpy(&_samples, &tmp_samples, sizeof(tmp_samples)); + pthread_mutex_unlock(&_samples_lock); +} + +int NavioADC::init() +{ + int ret; + + ret = DriverFramework::VirtDevObj::init(); + + if (ret != PX4_OK) { + PX4_ERR("init failed"); + return ret; + } + + return PX4_OK; +} + +int NavioADC::devIOCTL(unsigned long request, unsigned long arg) +{ + return -ENOTTY; +} + +ssize_t NavioADC::devRead(void *buf, size_t count) +{ + const size_t maxsize = sizeof(_samples); + int ret; + + if (count > maxsize) + count = maxsize; + + ret = pthread_mutex_trylock(&_samples_lock); + if (ret != 0) + return 0; + + memcpy(buf, &_samples, count); + pthread_mutex_unlock(&_samples_lock); + + return count; +} + +int NavioADC::read_channel(struct adc_msg_s *adc_msg, int channel) +{ + char buffer[11]; /* 32bit max INT has maximum 10 chars */ + char channel_path[sizeof(ADC_SYSFS_PATH)]; + int fd; + int ret; + + if (channel < 0 || channel > 5) { + return -EINVAL; + } + + strncpy(channel_path, ADC_SYSFS_PATH, sizeof(ADC_SYSFS_PATH)); + channel_path[sizeof(ADC_SYSFS_PATH) - 2] += channel; + + fd = ::open(channel_path, O_RDONLY); + if (fd == -1) { + ret = errno; + PX4_ERR("read_channel: open: %s (%d)", strerror(ret), ret); + return ret; + } + + ret = ::read(fd, buffer, sizeof(buffer) - 1); + if (ret == -1) { + ret = errno; + PX4_ERR("read_channel: read: %s (%d)", strerror(ret), ret); + goto cleanup; + } else if (ret == 0) { + PX4_ERR("read_channel: read empty"); + ret = -EINVAL; + goto cleanup; + } + + buffer[ret] = 0; + + adc_msg->am_channel = channel; + adc_msg->am_data = strtol(buffer, NULL, 10); + ret = 0; + +cleanup: + ::close(fd); + + return ret; +} + +static NavioADC *instance = nullptr; + +int navio_adc_main(int argc, char *argv[]) +{ + int ret; + + if (argc < 2) { + PX4_WARN("usage: "); + return PX4_ERROR; + } + + if (!strcmp(argv[1], "start")) { + if (instance) { + PX4_WARN("already started"); + return PX4_OK; + } + + instance = new NavioADC; + if (!instance) { + PX4_WARN("not enough memory"); + return PX4_ERROR; + } + + if (instance->init() != PX4_OK) { + delete instance; + instance = nullptr; + PX4_WARN("init failed"); + return PX4_ERROR; + } + + return PX4_OK; + } else if (!strcmp(argv[1], "stop")) { + if (!instance) { + PX4_WARN("already stopped"); + return PX4_OK; + } + + delete instance; + instance = nullptr; + return PX4_OK; + } else if (!strcmp(argv[1], "test")) { + if (!instance) { + PX4_ERR("start first"); + return PX4_ERROR; + } + + struct adc_msg_s adc_msgs[ADC_MAX_CHAN]; + + ret = instance->devRead((char*)&adc_msgs, sizeof(adc_msgs)); + if (ret < 0) { + PX4_ERR("ret: %s (%d)\n", strerror(ret), ret); + return ret; + } else if (ret != sizeof(adc_msgs)) { + PX4_ERR("incomplete read: %d expected %d", ret, sizeof(adc_msgs)); + return ret; + } + + for (int i = 0; i < ADC_MAX_CHAN; ++i) { + PX4_INFO("chan: %d; value: %d", (int)adc_msgs[i].am_channel, + adc_msgs[i].am_data); + } + + return PX4_OK; + } else { + PX4_WARN("action (%s) not supported", argv[1]); + + return PX4_ERROR; + } + + return PX4_OK; + +} diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a6c0c54e69..ed76ed4690 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -554,7 +554,7 @@ Sensors::task_main() int ret = 0; if (!_hil_enabled) { -#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) +#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_BEBOP) adc_init(); #endif }