From 75cd41a7c1a3f6d21d512eb982f90aa7aa099a44 Mon Sep 17 00:00:00 2001 From: Staroselskii Georgii Date: Wed, 26 Nov 2014 15:03:06 +0300 Subject: [PATCH] AP_HAL_Linux: added NavioAnalogIn --- libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h | 1 + libraries/AP_HAL_Linux/HAL_Linux_Class.cpp | 5 + libraries/AP_HAL_Linux/NavioAnalogIn.cpp | 114 ++++++++++++++++++ libraries/AP_HAL_Linux/NavioAnalogIn.h | 48 ++++++++ 4 files changed, 168 insertions(+) create mode 100644 libraries/AP_HAL_Linux/NavioAnalogIn.cpp create mode 100644 libraries/AP_HAL_Linux/NavioAnalogIn.h diff --git a/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h b/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h index a792710d44..a7b0c3bbb2 100644 --- a/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h +++ b/libraries/AP_HAL_Linux/AP_HAL_Linux_Private.h @@ -11,6 +11,7 @@ #include "I2CDriver.h" #include "SPIDriver.h" #include "AnalogIn.h" +#include "NavioAnalogIn.h" #include "Storage.h" #include "GPIO.h" #include "RCInput.h" diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index 9c1c3bd900..aeaf6e9199 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -26,7 +26,11 @@ static LinuxUARTDriver uartCDriver(false); static LinuxSemaphore i2cSemaphore; static LinuxI2CDriver i2cDriver(&i2cSemaphore, "/dev/i2c-1"); static LinuxSPIDeviceManager spiDeviceManager; +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO +static NavioAnalogIn analogIn; +#else static LinuxAnalogIn analogIn; +#endif /* select between FRAM and FS @@ -145,6 +149,7 @@ void HAL_Linux::init(int argc,char* const argv[]) const rcin->init(NULL); uartA->begin(115200); spi->init(NULL); + analogin->init(NULL); utilInstance.init(argc, argv); } diff --git a/libraries/AP_HAL_Linux/NavioAnalogIn.cpp b/libraries/AP_HAL_Linux/NavioAnalogIn.cpp new file mode 100644 index 0000000000..4be80fe99b --- /dev/null +++ b/libraries/AP_HAL_Linux/NavioAnalogIn.cpp @@ -0,0 +1,114 @@ +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX + +#include "NavioAnalogIn.h" + +#define NAVIO_ANALOGIN_DEBUG 0 +#if NAVIO_ANALOGIN_DEBUG +#include +#define debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) +#define error(fmt, args ...) do {fprintf(stderr,"%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) +#else +#define debug(fmt, args ...) +#define error(fmt, args ...) +#endif + +NavioAnalogSource::NavioAnalogSource(int16_t pin): + _pin(pin), + _value(0.0f) +{ +} + +void NavioAnalogSource::set_pin(uint8_t pin) +{ + if (_pin == pin) { + return; + } + _pin = pin; +} + +float NavioAnalogSource::read_average() +{ + return read_latest(); +} + +float NavioAnalogSource::read_latest() +{ + return _value; +} + +float NavioAnalogSource::voltage_average() +{ + return _value; +} + +float NavioAnalogSource::voltage_latest() +{ + return _value; +} + +float NavioAnalogSource::voltage_average_ratiometric() +{ + return _value; +} + +extern const AP_HAL::HAL& hal; + +NavioAnalogIn::NavioAnalogIn() +{ + _adc = new AP_ADC_ADS1115(); + _channels_number = _adc->get_channels_number(); +} + +AP_HAL::AnalogSource* NavioAnalogIn::channel(int16_t pin) +{ + for (uint8_t j = 0; j < _channels_number; j++) { + if (_channels[j] == NULL) { + _channels[j] = new NavioAnalogSource(pin); + return _channels[j]; + } + } + + hal.console->println("Out of analog channels"); + return NULL; +} + +void NavioAnalogIn::init(void* implspecific) +{ + _adc->init(); + hal.scheduler->suspend_timer_procs(); + hal.scheduler->register_timer_process( AP_HAL_MEMBERPROC(&NavioAnalogIn::_update)); + hal.scheduler->resume_timer_procs(); +} + +void NavioAnalogIn::_update() +{ + if (hal.scheduler->micros() - _last_update_timestamp < 100000) { + return; + } + + adc_report_s reports[NAVIO_ADC_MAX_CHANNELS]; + + size_t rc = _adc->read(reports, 6); + + for (size_t i = 0; i < rc; i++) { + for (uint8_t j=0; j < rc; j++) { + NavioAnalogSource *source = _channels[j]; + +#if 0 + if (source != NULL) { + fprintf(stderr, "pin: %d id: %d data: %.3f\n", source->_pin, reports[i].id, reports[i].data); + } +#endif + + if (source != NULL && reports[i].id == source->_pin) { + source->_value = reports[i].data / 1000; + } + } + } + + _last_update_timestamp = hal.scheduler->micros(); +} + +#endif diff --git a/libraries/AP_HAL_Linux/NavioAnalogIn.h b/libraries/AP_HAL_Linux/NavioAnalogIn.h new file mode 100644 index 0000000000..51d595af72 --- /dev/null +++ b/libraries/AP_HAL_Linux/NavioAnalogIn.h @@ -0,0 +1,48 @@ +#ifndef __NavioAnalogIn_H__ +#define __NavioAnalogIn_H__ + +#include +#include + +#define NAVIO_ADC_MAX_CHANNELS 6 + +class NavioAnalogSource: public AP_HAL::AnalogSource { +public: + friend class NavioAnalogIn; + NavioAnalogSource(int16_t pin); + float read_average(); + float read_latest(); + void set_pin(uint8_t p); + void set_stop_pin(uint8_t p) {} + void set_settle_time(uint16_t settle_time_ms){} + float voltage_average(); + float voltage_latest(); + float voltage_average_ratiometric(); +private: + int16_t _pin; + float _value; +}; + +class NavioAnalogIn: public AP_HAL::AnalogIn { +public: + NavioAnalogIn(); + void init(void* implspecific); + AP_HAL::AnalogSource* channel(int16_t n); + + /* Board voltage is not available */ + float board_voltage(void) + { + return 0.0f; + } +private: + AP_ADC_ADS1115 *_adc; + NavioAnalogSource *_channels[NAVIO_ADC_MAX_CHANNELS]; + + uint8_t _channels_number; + + void _update(); + + uint32_t _last_update_timestamp; +}; + +#endif