ardupilot/libraries/AP_HAL_Linux/AnalogIn_IIO.cpp

128 lines
2.4 KiB
C++
Raw Normal View History

#include "AnalogIn_IIO.h"
#include <AP_HAL/AP_HAL.h>
extern const AP_HAL::HAL &hal;
const char* AnalogSource_IIO::analog_sources[] = {
"in_voltage0_raw",
"in_voltage1_raw",
"in_voltage2_raw",
"in_voltage3_raw",
"in_voltage4_raw",
"in_voltage5_raw",
"in_voltage6_raw",
"in_voltage7_raw",
};
AnalogSource_IIO::AnalogSource_IIO(int16_t pin, float initial_value, float voltage_scaling) :
_pin(pin),
_value(initial_value),
_voltage_scaling(voltage_scaling),
_sum_value(0),
_sum_count(0),
_pin_fd(-1)
{
init_pins();
select_pin();
}
void AnalogSource_IIO::init_pins(void)
{
char buf[100];
for (unsigned int i = 0; i < ARRAY_SIZE(AnalogSource_IIO::analog_sources); i++) {
// Construct the path by appending strings
strncpy(buf, IIO_ANALOG_IN_DIR, sizeof(buf));
strncat(buf, AnalogSource_IIO::analog_sources[i], sizeof(buf) - strlen(buf) - 1);
fd_analog_sources[i] = open(buf, O_RDONLY | O_NONBLOCK | O_CLOEXEC);
}
}
/*
2016-05-12 14:03:45 -03:00
selects a different file descriptor among in the fd_analog_sources array
*/
void AnalogSource_IIO::select_pin(void)
{
_pin_fd = fd_analog_sources[_pin];
}
float AnalogSource_IIO::read_average()
{
read_latest();
WITH_SEMAPHORE(_semaphore);
if (_sum_count == 0) {
return _value;
}
_value = _sum_value / _sum_count;
_sum_value = 0;
_sum_count = 0;
return _value;
}
float AnalogSource_IIO::read_latest()
{
char sbuf[10];
if (_pin_fd == -1) {
_latest = 0;
return 0;
}
memset(sbuf, 0, sizeof(sbuf));
if (pread(_pin_fd, sbuf, sizeof(sbuf) - 1, 0) < 0) {
_latest = 0;
return 0;
}
WITH_SEMAPHORE(_semaphore);
_latest = atoi(sbuf) * _voltage_scaling;
_sum_value += _latest;
_sum_count++;
return _latest;
}
// output is in volts
float AnalogSource_IIO::voltage_average()
{
return read_average();
}
float AnalogSource_IIO::voltage_latest()
{
read_latest();
return _latest;
}
bool AnalogSource_IIO::set_pin(uint8_t pin)
{
if (_pin == pin) {
return true;
}
WITH_SEMAPHORE(_semaphore);
_pin = pin;
_sum_value = 0;
_sum_count = 0;
_latest = 0;
_value = 0;
select_pin();
return true;
}
AnalogIn_IIO::AnalogIn_IIO()
{}
void AnalogIn_IIO::init()
{}
AP_HAL::AnalogSource* AnalogIn_IIO::channel(int16_t pin) {
return new AnalogSource_IIO(pin, 0.0f, IIO_VOLTAGE_SCALING);
}