diff --git a/libraries/AP_ADC/AP_ADC_ADS1115.cpp b/libraries/AP_ADC/AP_ADC_ADS1115.cpp index f89fc6971b..f1a35412e5 100644 --- a/libraries/AP_ADC/AP_ADC_ADS1115.cpp +++ b/libraries/AP_ADC/AP_ADC_ADS1115.cpp @@ -123,12 +123,9 @@ AP_ADC_ADS1115::~AP_ADC_ADS1115() bool AP_ADC_ADS1115::init() { - hal.scheduler->suspend_timer_procs(); - _gain = ADS1115_PGA_4P096; - hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_ADC_ADS1115::_update, void)); - hal.scheduler->resume_timer_procs(); + _dev->register_periodic_callback(100000, FUNCTOR_BIND_MEMBER(&AP_ADC_ADS1115::_update, bool)); return true; } @@ -198,39 +195,25 @@ float AP_ADC_ADS1115::_convert_register_data_to_mv(int16_t word) const return (float) word * pga; } -void AP_ADC_ADS1115::_update() +bool AP_ADC_ADS1115::_update() { - /* TODO: make update rate configurable */ - if (AP_HAL::micros() - _last_update_timestamp < 100000) { - return; - } - uint8_t config[2]; be16_t val; - if (!_dev->get_semaphore()->take_nonblocking()) { - return; - } - if (!_dev->read_registers(ADS1115_RA_CONFIG, config, sizeof(config))) { error("_dev->read_registers failed in ADS1115"); - _dev->get_semaphore()->give(); - return; + return true; } /* check rdy bit */ if ((config[1] & 0x80) != 0x80 ) { - _dev->get_semaphore()->give(); - return; + return true; } if (!_dev->read_registers(ADS1115_RA_CONVERSION, (uint8_t *)&val, sizeof(val))) { - _dev->get_semaphore()->give(); - return; + return true; } - _dev->get_semaphore()->give(); - float sample = _convert_register_data_to_mv(be16toh(val)); _samples[_channel_to_read].data = sample; @@ -240,5 +223,5 @@ void AP_ADC_ADS1115::_update() _channel_to_read = (_channel_to_read + 1) % _channels_number; _start_conversion(_channel_to_read); - _last_update_timestamp = AP_HAL::micros(); + return true; } diff --git a/libraries/AP_ADC/AP_ADC_ADS1115.h b/libraries/AP_ADC/AP_ADC_ADS1115.h index 98e6544875..2f49a9dc8a 100644 --- a/libraries/AP_ADC/AP_ADC_ADS1115.h +++ b/libraries/AP_ADC/AP_ADC_ADS1115.h @@ -36,7 +36,7 @@ private: int _channel_to_read; adc_report_s *_samples; - void _update(); + bool _update(); bool _start_conversion(uint8_t channel); float _convert_register_data_to_mv(int16_t word) const;