ardupilot/libraries/AP_HAL_Linux/AnalogIn_ADS1115.cpp

94 lines
1.8 KiB
C++
Raw Normal View History

#include "AnalogIn_ADS1115.h"
2014-11-26 08:03:06 -04:00
AnalogSource_ADS1115::AnalogSource_ADS1115(int16_t pin):
2014-11-26 08:03:06 -04:00
_pin(pin),
_value(0.0f)
{
}
bool AnalogSource_ADS1115::set_pin(uint8_t pin)
2014-11-26 08:03:06 -04:00
{
if (_pin == pin) {
return true;
2014-11-26 08:03:06 -04:00
}
_pin = pin;
return true;
2014-11-26 08:03:06 -04:00
}
float AnalogSource_ADS1115::read_average()
{
2014-11-26 08:03:06 -04:00
return read_latest();
}
float AnalogSource_ADS1115::read_latest()
2014-11-26 08:03:06 -04:00
{
return _value;
}
float AnalogSource_ADS1115::voltage_average()
2014-11-26 08:03:06 -04:00
{
return _value;
}
float AnalogSource_ADS1115::voltage_latest()
2014-11-26 08:03:06 -04:00
{
return _value;
}
float AnalogSource_ADS1115::voltage_average_ratiometric()
2014-11-26 08:03:06 -04:00
{
return _value;
}
extern const AP_HAL::HAL &hal;
2014-11-26 08:03:06 -04:00
AnalogIn_ADS1115::AnalogIn_ADS1115()
2014-11-26 08:03:06 -04:00
{
_adc = new AP_ADC_ADS1115();
_channels_number = _adc->get_channels_number();
}
AP_HAL::AnalogSource* AnalogIn_ADS1115::channel(int16_t pin)
2014-11-26 08:03:06 -04:00
{
WITH_SEMAPHORE(_semaphore);
2014-11-26 08:03:06 -04:00
for (uint8_t j = 0; j < _channels_number; j++) {
if (_channels[j] == nullptr) {
_channels[j] = new AnalogSource_ADS1115(pin);
2014-11-26 08:03:06 -04:00
return _channels[j];
}
}
hal.console->printf("Out of analog channels\n");
return nullptr;
2014-11-26 08:03:06 -04:00
}
void AnalogIn_ADS1115::init()
2014-11-26 08:03:06 -04:00
{
_adc->init();
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AnalogIn_ADS1115::_update, void));
2014-11-26 08:03:06 -04:00
}
void AnalogIn_ADS1115::_update()
2014-11-26 08:03:06 -04:00
{
if (AP_HAL::micros() - _last_update_timestamp < 100000) {
2014-11-26 08:03:06 -04:00
return;
}
adc_report_s reports[ADS1115_ADC_MAX_CHANNELS];
2014-11-26 08:03:06 -04:00
size_t rc = _adc->read(reports, 6);
for (size_t i = 0; i < rc; i++) {
for (uint8_t j=0; j < rc; j++) {
AnalogSource_ADS1115 *source = _channels[j];
2014-11-26 08:03:06 -04:00
if (source != nullptr && reports[i].id == source->_pin) {
2014-11-26 08:03:06 -04:00
source->_value = reports[i].data / 1000;
}
}
}
_last_update_timestamp = AP_HAL::micros();
2014-11-26 08:03:06 -04:00
}