mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-01 21:48:28 -04:00
AP_HAL_Linux: ADS1115: rename classes to follow other names
While at it, remove some dead/commented-out code.
This commit is contained in:
parent
4562c0cffc
commit
87e7704b1c
@ -4,23 +4,13 @@
|
|||||||
|
|
||||||
#include "AnalogIn_ADS1115.h"
|
#include "AnalogIn_ADS1115.h"
|
||||||
|
|
||||||
#define ADS1115_ANALOGIN_DEBUG 0
|
AnalogSource_ADS1115::AnalogSource_ADS1115(int16_t pin):
|
||||||
#if ADS1115_ANALOGIN_DEBUG
|
|
||||||
#include <cstdio>
|
|
||||||
#define debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
|
|
||||||
#define error(fmt, args ...) do {fprintf(stderr,"%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
|
|
||||||
#else
|
|
||||||
#define debug(fmt, args ...)
|
|
||||||
#define error(fmt, args ...)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
ADS1115AnalogSource::ADS1115AnalogSource(int16_t pin):
|
|
||||||
_pin(pin),
|
_pin(pin),
|
||||||
_value(0.0f)
|
_value(0.0f)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
void ADS1115AnalogSource::set_pin(uint8_t pin)
|
void AnalogSource_ADS1115::set_pin(uint8_t pin)
|
||||||
{
|
{
|
||||||
if (_pin == pin) {
|
if (_pin == pin) {
|
||||||
return;
|
return;
|
||||||
@ -28,44 +18,44 @@ void ADS1115AnalogSource::set_pin(uint8_t pin)
|
|||||||
_pin = pin;
|
_pin = pin;
|
||||||
}
|
}
|
||||||
|
|
||||||
float ADS1115AnalogSource::read_average()
|
float AnalogSource_ADS1115::read_average()
|
||||||
{
|
{
|
||||||
return read_latest();
|
return read_latest();
|
||||||
}
|
}
|
||||||
|
|
||||||
float ADS1115AnalogSource::read_latest()
|
float AnalogSource_ADS1115::read_latest()
|
||||||
{
|
{
|
||||||
return _value;
|
return _value;
|
||||||
}
|
}
|
||||||
|
|
||||||
float ADS1115AnalogSource::voltage_average()
|
float AnalogSource_ADS1115::voltage_average()
|
||||||
{
|
{
|
||||||
return _value;
|
return _value;
|
||||||
}
|
}
|
||||||
|
|
||||||
float ADS1115AnalogSource::voltage_latest()
|
float AnalogSource_ADS1115::voltage_latest()
|
||||||
{
|
{
|
||||||
return _value;
|
return _value;
|
||||||
}
|
}
|
||||||
|
|
||||||
float ADS1115AnalogSource::voltage_average_ratiometric()
|
float AnalogSource_ADS1115::voltage_average_ratiometric()
|
||||||
{
|
{
|
||||||
return _value;
|
return _value;
|
||||||
}
|
}
|
||||||
|
|
||||||
extern const AP_HAL::HAL& hal;
|
extern const AP_HAL::HAL &hal;
|
||||||
|
|
||||||
ADS1115AnalogIn::ADS1115AnalogIn()
|
AnalogIn_ADS1115::AnalogIn_ADS1115()
|
||||||
{
|
{
|
||||||
_adc = new AP_ADC_ADS1115();
|
_adc = new AP_ADC_ADS1115();
|
||||||
_channels_number = _adc->get_channels_number();
|
_channels_number = _adc->get_channels_number();
|
||||||
}
|
}
|
||||||
|
|
||||||
AP_HAL::AnalogSource* ADS1115AnalogIn::channel(int16_t pin)
|
AP_HAL::AnalogSource* AnalogIn_ADS1115::channel(int16_t pin)
|
||||||
{
|
{
|
||||||
for (uint8_t j = 0; j < _channels_number; j++) {
|
for (uint8_t j = 0; j < _channels_number; j++) {
|
||||||
if (_channels[j] == NULL) {
|
if (_channels[j] == NULL) {
|
||||||
_channels[j] = new ADS1115AnalogSource(pin);
|
_channels[j] = new AnalogSource_ADS1115(pin);
|
||||||
return _channels[j];
|
return _channels[j];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@ -74,15 +64,16 @@ AP_HAL::AnalogSource* ADS1115AnalogIn::channel(int16_t pin)
|
|||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ADS1115AnalogIn::init()
|
void AnalogIn_ADS1115::init()
|
||||||
{
|
{
|
||||||
_adc->init();
|
_adc->init();
|
||||||
|
|
||||||
hal.scheduler->suspend_timer_procs();
|
hal.scheduler->suspend_timer_procs();
|
||||||
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&ADS1115AnalogIn::_update, void));
|
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AnalogIn_ADS1115::_update, void));
|
||||||
hal.scheduler->resume_timer_procs();
|
hal.scheduler->resume_timer_procs();
|
||||||
}
|
}
|
||||||
|
|
||||||
void ADS1115AnalogIn::_update()
|
void AnalogIn_ADS1115::_update()
|
||||||
{
|
{
|
||||||
if (AP_HAL::micros() - _last_update_timestamp < 100000) {
|
if (AP_HAL::micros() - _last_update_timestamp < 100000) {
|
||||||
return;
|
return;
|
||||||
@ -94,13 +85,7 @@ void ADS1115AnalogIn::_update()
|
|||||||
|
|
||||||
for (size_t i = 0; i < rc; i++) {
|
for (size_t i = 0; i < rc; i++) {
|
||||||
for (uint8_t j=0; j < rc; j++) {
|
for (uint8_t j=0; j < rc; j++) {
|
||||||
ADS1115AnalogSource *source = _channels[j];
|
AnalogSource_ADS1115 *source = _channels[j];
|
||||||
|
|
||||||
#if 0
|
|
||||||
if (source != NULL) {
|
|
||||||
fprintf(stderr, "pin: %d id: %d data: %.3f\n", source->_pin, reports[i].id, reports[i].data);
|
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
||||||
if (source != NULL && reports[i].id == source->_pin) {
|
if (source != NULL && reports[i].id == source->_pin) {
|
||||||
source->_value = reports[i].data / 1000;
|
source->_value = reports[i].data / 1000;
|
||||||
|
@ -1,15 +1,14 @@
|
|||||||
#ifndef __ADS1115AnalogIn_H__
|
#pragma once
|
||||||
#define __ADS1115AnalogIn_H__
|
|
||||||
|
|
||||||
#include "AP_HAL_Linux.h"
|
#include "AP_HAL_Linux.h"
|
||||||
#include <AP_ADC/AP_ADC.h>
|
#include <AP_ADC/AP_ADC.h>
|
||||||
|
|
||||||
#define ADS1115_ADC_MAX_CHANNELS 6
|
#define ADS1115_ADC_MAX_CHANNELS 6
|
||||||
|
|
||||||
class ADS1115AnalogSource: public AP_HAL::AnalogSource {
|
class AnalogSource_ADS1115: public AP_HAL::AnalogSource {
|
||||||
public:
|
public:
|
||||||
friend class ADS1115AnalogIn;
|
friend class AnalogIn_ADS1115;
|
||||||
ADS1115AnalogSource(int16_t pin);
|
AnalogSource_ADS1115(int16_t pin);
|
||||||
float read_average();
|
float read_average();
|
||||||
float read_latest();
|
float read_latest();
|
||||||
void set_pin(uint8_t p);
|
void set_pin(uint8_t p);
|
||||||
@ -23,26 +22,22 @@ private:
|
|||||||
float _value;
|
float _value;
|
||||||
};
|
};
|
||||||
|
|
||||||
class ADS1115AnalogIn: public AP_HAL::AnalogIn {
|
class AnalogIn_ADS1115: public AP_HAL::AnalogIn {
|
||||||
public:
|
public:
|
||||||
ADS1115AnalogIn();
|
AnalogIn_ADS1115();
|
||||||
void init();
|
void init();
|
||||||
AP_HAL::AnalogSource* channel(int16_t n);
|
AP_HAL::AnalogSource* channel(int16_t n);
|
||||||
|
|
||||||
/* Board voltage is not available */
|
/* Board voltage is not available */
|
||||||
float board_voltage(void)
|
float board_voltage(void)
|
||||||
{
|
{
|
||||||
return 0.0f;
|
return 0.0f;
|
||||||
}
|
}
|
||||||
private:
|
private:
|
||||||
AP_ADC_ADS1115 *_adc;
|
|
||||||
ADS1115AnalogSource *_channels[ADS1115_ADC_MAX_CHANNELS];
|
|
||||||
|
|
||||||
uint8_t _channels_number;
|
uint8_t _channels_number;
|
||||||
|
|
||||||
void _update();
|
void _update();
|
||||||
|
|
||||||
uint32_t _last_update_timestamp;
|
AP_ADC_ADS1115 *_adc;
|
||||||
|
AnalogSource_ADS1115 *_channels[ADS1115_ADC_MAX_CHANNELS];
|
||||||
|
uint32_t _last_update_timestamp;
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif
|
|
||||||
|
@ -77,7 +77,7 @@ static SPIDeviceManager spiDeviceManager;
|
|||||||
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
|
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 || \
|
||||||
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \
|
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH || \
|
||||||
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
|
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI
|
||||||
static ADS1115AnalogIn analogIn;
|
static AnalogIn_ADS1115 analogIn;
|
||||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
|
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_RASPILOT
|
||||||
static RaspilotAnalogIn analogIn;
|
static RaspilotAnalogIn analogIn;
|
||||||
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
|
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
|
||||||
|
Loading…
Reference in New Issue
Block a user