From 85c0c9819478f9740646d9ce9664a3f49ea6d7ba Mon Sep 17 00:00:00 2001 From: Lucas De Marchi Date: Mon, 11 Jul 2016 14:44:15 -0300 Subject: [PATCH] AP_ADC: ADS1115: fix driver after conversion to I2CDevice In _start_conversion(), the check for return code of _dev->transfer() was inverted. The structure also needs to be PACKED, otherwise there will be a hole in the middle. Fix these issues and use be16_t where it makes sense. In read() we need to check for the second byte of config register, so either make it an array of uint8_t or convert from big endian to host endianness. It's simpler to leave it as it was, accessing just the first byte. Also the conversion value is in be16 type an needs to be converted to host endiannes, not the opposite. Fix bus number: all boards that use it expect it to be on bus 1, not 0. --- libraries/AP_ADC/AP_ADC_ADS1115.cpp | 39 +++++++++++++---------------- 1 file changed, 18 insertions(+), 21 deletions(-) diff --git a/libraries/AP_ADC/AP_ADC_ADS1115.cpp b/libraries/AP_ADC/AP_ADC_ADS1115.cpp index 68ec2f82eb..6cbf60f6f5 100644 --- a/libraries/AP_ADC/AP_ADC_ADS1115.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS1115.cpp @@ -7,7 +7,9 @@ #define ADS1115_ADDRESS_ADDR_VDD 0x49 // address pin high (VCC) #define ADS1115_ADDRESS_ADDR_SDA 0x4A // address pin tied to SDA pin #define ADS1115_ADDRESS_ADDR_SCL 0x4B // address pin tied to SCL pin -#define ADS1115_DEFAULT_ADDRESS ADS1115_ADDRESS_ADDR_GND + +#define ADS1115_I2C_ADDR ADS1115_ADDRESS_ADDR_GND +#define ADS1115_I2C_BUS 1 #define ADS1115_RA_CONVERSION 0x00 #define ADS1115_RA_CONFIG 0x01 @@ -106,9 +108,9 @@ static const uint16_t mux_table[ADS1115_CHANNELS_COUNT] = { }; -AP_ADC_ADS1115::AP_ADC_ADS1115(): - _dev(hal.i2c_mgr->get_device(0, ADS1115_DEFAULT_ADDRESS)), - _channel_to_read(0) +AP_ADC_ADS1115::AP_ADC_ADS1115() + : _dev(hal.i2c_mgr->get_device(ADS1115_I2C_BUS, ADS1115_I2C_ADDR)) + , _channel_to_read(0) { _samples = new adc_report_s[_channels_number]; } @@ -127,22 +129,17 @@ bool AP_ADC_ADS1115::init() bool AP_ADC_ADS1115::_start_conversion(uint8_t channel) { - struct { + struct PACKED { uint8_t reg; - uint16_t w; + be16_t val; } config; - config.w = ADS1115_OS_ACTIVE | _gain | mux_table[channel] | - ADS1115_MODE_SINGLESHOT | ADS1115_COMP_QUE_DISABLE | ADS1115_RATE_250; - config.reg = ADS1115_RA_CONFIG; - config.w = htobe16(config.w); + config.val = htobe16(ADS1115_OS_ACTIVE | _gain | mux_table[channel] | + ADS1115_MODE_SINGLESHOT | ADS1115_COMP_QUE_DISABLE | + ADS1115_RATE_250); - if (_dev->transfer((uint8_t *)&config, sizeof(config), nullptr, 0)) { - return false; - } - - return true; + return _dev->transfer((uint8_t *)&config, sizeof(config), nullptr, 0); } size_t AP_ADC_ADS1115::read(adc_report_s *report, size_t length) const @@ -202,32 +199,32 @@ void AP_ADC_ADS1115::_update() return; } - uint16_t word, config; + uint8_t config[2]; + be16_t val; if (!_dev->get_semaphore()->take_nonblocking()) { return; } - if (!_dev->read_registers(ADS1115_RA_CONFIG, (uint8_t *)&config, sizeof(config))) { + if (!_dev->read_registers(ADS1115_RA_CONFIG, config, sizeof(config))) { error("i2c->read_registers failed in ADS1115"); _dev->get_semaphore()->give(); return; } /* check rdy bit */ - if ((config & 0x80) != 0x80 ) { + if ((config[1] & 0x80) != 0x80 ) { _dev->get_semaphore()->give(); return; } - if (!_dev->read_registers(ADS1115_RA_CONVERSION, (uint8_t *)&word, sizeof(word))) { + if (!_dev->read_registers(ADS1115_RA_CONVERSION, (uint8_t *)&val, sizeof(val))) { _dev->get_semaphore()->give(); return; } - word = htobe16(word); - float sample = _convert_register_data_to_mv(word); + float sample = _convert_register_data_to_mv(be16toh(val)); _samples[_channel_to_read].data = sample; _samples[_channel_to_read].id = _channel_to_read;