490841a814
By opening with O_CLOEXEC we make sure we don't leak the file descriptor when we are exec'ing or calling out subprograms. Right now we currently don't do it so there's no harm, but it's good practice in Linux to have it.
126 lines
2.3 KiB
C++
126 lines
2.3 KiB
C++
#include <cstdio>
|
|
#include <cstdlib>
|
|
#include <errno.h>
|
|
#include <unistd.h>
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#include "AnalogIn_Navio2.h"
|
|
|
|
static const AP_HAL::HAL &hal = AP_HAL::get_HAL();
|
|
|
|
#define ADC_BASE_PATH "/sys/kernel/rcio/adc"
|
|
|
|
void AnalogSource_Navio2::set_channel(uint8_t pin)
|
|
{
|
|
char *channel_path;
|
|
|
|
if (pin == ANALOG_INPUT_NONE) {
|
|
return;
|
|
}
|
|
|
|
if (asprintf(&channel_path, "%s/ch%d", ADC_BASE_PATH, pin) == -1) {
|
|
AP_HAL::panic("asprintf failed\n");
|
|
}
|
|
|
|
if (_fd >= 0) {
|
|
::close(_fd);
|
|
}
|
|
|
|
_fd = ::open(channel_path, O_RDONLY|O_CLOEXEC);
|
|
|
|
if (_fd < 0) {
|
|
hal.console->printf("%s not opened: %s\n", channel_path, strerror(errno));
|
|
}
|
|
|
|
free(channel_path);
|
|
}
|
|
|
|
AnalogSource_Navio2::AnalogSource_Navio2(uint8_t pin)
|
|
: _pin(pin)
|
|
{
|
|
set_channel(pin);
|
|
}
|
|
|
|
void AnalogSource_Navio2::set_pin(uint8_t pin)
|
|
{
|
|
if (_pin == pin) {
|
|
return;
|
|
}
|
|
|
|
set_channel(pin);
|
|
|
|
_pin = pin;
|
|
}
|
|
|
|
float AnalogSource_Navio2::read_average()
|
|
{
|
|
return read_latest();
|
|
}
|
|
|
|
float AnalogSource_Navio2::read_latest()
|
|
{
|
|
return voltage_average();
|
|
}
|
|
|
|
float AnalogSource_Navio2::voltage_average()
|
|
{
|
|
char buffer[12];
|
|
|
|
if (pread(_fd, buffer, sizeof(buffer) - 1, 0) <= 0) {
|
|
/* Don't log fails since this could spam the console */
|
|
return -1.0f;
|
|
}
|
|
|
|
/* Avoid overriding NULL char at the end of the string */
|
|
buffer[sizeof(buffer) - 1] = '\0';
|
|
|
|
_value = atoi(buffer) / 1000.0f;
|
|
|
|
return _value;
|
|
}
|
|
|
|
float AnalogSource_Navio2::voltage_latest()
|
|
{
|
|
read_latest();
|
|
return _value;
|
|
}
|
|
|
|
float AnalogSource_Navio2::voltage_average_ratiometric()
|
|
{
|
|
return voltage_average();
|
|
}
|
|
|
|
AnalogIn_Navio2::AnalogIn_Navio2()
|
|
{
|
|
}
|
|
|
|
float AnalogIn_Navio2::board_voltage(void)
|
|
{
|
|
return _board_voltage_pin->voltage_average();
|
|
}
|
|
|
|
float AnalogIn_Navio2::servorail_voltage(void)
|
|
{
|
|
return _servorail_pin->voltage_average();
|
|
}
|
|
|
|
AP_HAL::AnalogSource *AnalogIn_Navio2::channel(int16_t pin)
|
|
{
|
|
for (uint8_t j = 0; j < _channels_number; j++) {
|
|
if (_channels[j] == nullptr) {
|
|
_channels[j] = new AnalogSource_Navio2(pin);
|
|
return _channels[j];
|
|
}
|
|
}
|
|
|
|
hal.console->println("Out of analog channels");
|
|
return nullptr;
|
|
}
|
|
|
|
void AnalogIn_Navio2::init()
|
|
{
|
|
_board_voltage_pin = channel(0);
|
|
_servorail_pin = channel(1);
|
|
}
|