mirror of https://github.com/ArduPilot/ardupilot
HAL_Linux: mv AnalogIn_PXF to AnalogIn_IIO
Initial implementation used for the PXF moved for a wider use. The implementation uses the Linux Industrial I/O Subsystem (IIO) to get Analog Input.
This commit is contained in:
parent
4820e0b2c7
commit
546fbd1f25
|
@ -14,7 +14,7 @@
|
|||
#include "AnalogIn.h"
|
||||
#include "AnalogIn_ADS1115.h"
|
||||
#include "RaspilotAnalogIn.h"
|
||||
#include "AnalogIn_PXF.h"
|
||||
#include "AnalogIn_IIO.h"
|
||||
#include "Storage.h"
|
||||
#include "GPIO.h"
|
||||
#include "RCInput.h"
|
||||
|
|
|
@ -2,13 +2,14 @@
|
|||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
||||
#include "AnalogIn_PXF.h"
|
||||
#include "AnalogIn_IIO.h"
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
|
||||
|
||||
const char* PXFAnalogSource::analog_sources[PXF_ANALOG_IN_COUNT] = {
|
||||
const char* IIOAnalogSource::analog_sources[IIO_ANALOG_IN_COUNT] = {
|
||||
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
|
||||
"in_voltage0_raw",
|
||||
"in_voltage1_raw",
|
||||
"in_voltage2_raw",
|
||||
|
@ -17,9 +18,12 @@ const char* PXFAnalogSource::analog_sources[PXF_ANALOG_IN_COUNT] = {
|
|||
"in_voltage5_raw",
|
||||
"in_voltage6_raw",
|
||||
"in_voltage7_raw",
|
||||
#else
|
||||
"in_voltage0_raw",
|
||||
#endif
|
||||
};
|
||||
|
||||
PXFAnalogSource::PXFAnalogSource(int16_t pin, float initial_value) :
|
||||
IIOAnalogSource::IIOAnalogSource(int16_t pin, float initial_value) :
|
||||
_pin(pin),
|
||||
_value(initial_value),
|
||||
_sum_value(0),
|
||||
|
@ -29,7 +33,7 @@ PXFAnalogSource::PXFAnalogSource(int16_t pin, float initial_value) :
|
|||
reopen_pin();
|
||||
}
|
||||
|
||||
void PXFAnalogSource::reopen_pin(void)
|
||||
void IIOAnalogSource::reopen_pin(void)
|
||||
{
|
||||
char buf[100];
|
||||
|
||||
|
@ -42,14 +46,14 @@ void PXFAnalogSource::reopen_pin(void)
|
|||
return;
|
||||
}
|
||||
|
||||
if (_pin > PXF_ANALOG_IN_COUNT) {
|
||||
if (_pin > IIO_ANALOG_IN_COUNT) {
|
||||
// invalid pin
|
||||
return;
|
||||
}
|
||||
|
||||
// Construct the path by appending strings
|
||||
strncpy(buf, PXF_ANALOG_IN_DIR, sizeof(buf));
|
||||
strncat(buf, PXFAnalogSource::analog_sources[_pin], sizeof(buf));
|
||||
strncpy(buf, IIO_ANALOG_IN_DIR, sizeof(buf));
|
||||
strncat(buf, IIOAnalogSource::analog_sources[_pin], sizeof(buf));
|
||||
|
||||
_pin_fd = open(buf, O_RDONLY | O_NONBLOCK);
|
||||
if (_pin_fd == -1) {
|
||||
|
@ -57,7 +61,7 @@ void PXFAnalogSource::reopen_pin(void)
|
|||
}
|
||||
}
|
||||
|
||||
float PXFAnalogSource::read_average()
|
||||
float IIOAnalogSource::read_average()
|
||||
{
|
||||
read_latest();
|
||||
if (_sum_count == 0) {
|
||||
|
@ -73,7 +77,7 @@ float PXFAnalogSource::read_average()
|
|||
return _value;
|
||||
}
|
||||
|
||||
float PXFAnalogSource::read_latest()
|
||||
float IIOAnalogSource::read_latest()
|
||||
{
|
||||
char sbuf[10];
|
||||
|
||||
|
@ -84,8 +88,11 @@ float PXFAnalogSource::read_latest()
|
|||
|
||||
memset(sbuf, 0, sizeof(sbuf));
|
||||
pread(_pin_fd, sbuf, sizeof(sbuf)-1, 0);
|
||||
|
||||
_latest = atoi(sbuf) * BBB_VOLTAGE_SCALING;
|
||||
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
|
||||
_latest = atoi(sbuf) * BBB_VOLTAGE_SCALING;
|
||||
#else
|
||||
_latest = atoi(sbuf)
|
||||
#endif
|
||||
_sum_value += _latest;
|
||||
_sum_count++;
|
||||
|
||||
|
@ -93,18 +100,18 @@ float PXFAnalogSource::read_latest()
|
|||
}
|
||||
|
||||
// output is in volts
|
||||
float PXFAnalogSource::voltage_average()
|
||||
float IIOAnalogSource::voltage_average()
|
||||
{
|
||||
return read_average();
|
||||
}
|
||||
|
||||
float PXFAnalogSource::voltage_latest()
|
||||
float IIOAnalogSource::voltage_latest()
|
||||
{
|
||||
read_latest();
|
||||
return _latest;
|
||||
}
|
||||
|
||||
void PXFAnalogSource::set_pin(uint8_t pin)
|
||||
void IIOAnalogSource::set_pin(uint8_t pin)
|
||||
{
|
||||
if (_pin == pin) {
|
||||
return;
|
||||
|
@ -122,21 +129,21 @@ void PXFAnalogSource::set_pin(uint8_t pin)
|
|||
hal.scheduler->resume_timer_procs();
|
||||
}
|
||||
|
||||
void PXFAnalogSource::set_stop_pin(uint8_t p)
|
||||
void IIOAnalogSource::set_stop_pin(uint8_t p)
|
||||
{}
|
||||
|
||||
void PXFAnalogSource::set_settle_time(uint16_t settle_time_ms)
|
||||
void IIOAnalogSource::set_settle_time(uint16_t settle_time_ms)
|
||||
{}
|
||||
|
||||
PXFAnalogIn::PXFAnalogIn()
|
||||
IIOAnalogIn::IIOAnalogIn()
|
||||
{}
|
||||
|
||||
void PXFAnalogIn::init()
|
||||
void IIOAnalogIn::init()
|
||||
{}
|
||||
|
||||
|
||||
AP_HAL::AnalogSource* PXFAnalogIn::channel(int16_t pin) {
|
||||
return new PXFAnalogSource(pin, 0);
|
||||
AP_HAL::AnalogSource* IIOAnalogIn::channel(int16_t pin) {
|
||||
return new IIOAnalogSource(pin, 0);
|
||||
}
|
||||
|
||||
#endif // CONFIG_HAL_BOARD
|
|
@ -1,6 +1,6 @@
|
|||
|
||||
#ifndef __AP_HAL_PXF_ANALOGIN_H__
|
||||
#define __AP_HAL_PXF_ANALOGIN_H__
|
||||
#ifndef __AP_HAL_IIO_ANALOGIN_H__
|
||||
#define __AP_HAL_IIO_ANALOGIN_H__
|
||||
|
||||
#include "AP_HAL_Linux.h"
|
||||
#include <AP_ADC/AP_ADC.h>
|
||||
|
@ -12,16 +12,20 @@
|
|||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
|
||||
#define PXF_ANALOG_IN_COUNT 8
|
||||
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF
|
||||
#define IIO_ANALOG_IN_COUNT 8
|
||||
// Note that echo BB-ADC cape should be loaded
|
||||
#define PXF_ANALOG_IN_DIR "/sys/bus/iio/devices/iio:device0/"
|
||||
#define IIO_ANALOG_IN_DIR "/sys/bus/iio/devices/iio:device0/"
|
||||
#define BBB_VOLTAGE_SCALING 0.00142602816
|
||||
#else
|
||||
#define IIO_ANALOG_IN_COUNT 8
|
||||
#define IIO_ANALOG_IN_DIR "/sys/bus/iio/devices/iio:device0/"
|
||||
#endif
|
||||
|
||||
class PXFAnalogSource : public AP_HAL::AnalogSource {
|
||||
class IIOAnalogSource : public AP_HAL::AnalogSource {
|
||||
public:
|
||||
friend class PXFAnalogIn;
|
||||
PXFAnalogSource(int16_t pin, float v);
|
||||
friend class IIOAnalogIn;
|
||||
IIOAnalogSource(int16_t pin, float v);
|
||||
float read_average();
|
||||
float read_latest();
|
||||
void set_pin(uint8_t p);
|
||||
|
@ -41,12 +45,12 @@ private:
|
|||
|
||||
void reopen_pin(void);
|
||||
|
||||
static const char *analog_sources[PXF_ANALOG_IN_COUNT];
|
||||
static const char *analog_sources[IIO_ANALOG_IN_COUNT];
|
||||
};
|
||||
|
||||
class PXFAnalogIn : public AP_HAL::AnalogIn {
|
||||
class IIOAnalogIn : public AP_HAL::AnalogIn {
|
||||
public:
|
||||
PXFAnalogIn();
|
||||
IIOAnalogIn();
|
||||
void init();
|
||||
AP_HAL::AnalogSource* channel(int16_t n);
|
||||
|
||||
|
@ -54,4 +58,4 @@ public:
|
|||
float board_voltage(void) { return 0.0f; }
|
||||
|
||||
};
|
||||
#endif // __AP_HAL_PXF_ANALOGIN_H__
|
||||
#endif // __AP_HAL_IIO_ANALOGIN_H__
|
|
@ -83,7 +83,7 @@ static RaspilotAnalogIn analogIn;
|
|||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
|
||||
static Empty::AnalogIn analogIn;
|
||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLE
|
||||
static PXFAnalogIn analogIn;
|
||||
static IIOAnalogIn analogIn;
|
||||
#else
|
||||
static AnalogIn analogIn;
|
||||
#endif
|
||||
|
|
Loading…
Reference in New Issue