Browse Source

AP_HAL_Linux: Add AnalogIn_ADS1115, common code

This commit provides a single abstraction for both
Erle-Brain 2 and Navio boards.
master
Víctor Mayoral Vilches 9 years ago committed by Andrew Tridgell
parent
commit
b1a53c383c
  1. 36
      libraries/AP_HAL_Linux/AnalogIn_ADS1115.cpp
  2. 18
      libraries/AP_HAL_Linux/AnalogIn_ADS1115.h

36
libraries/AP_HAL_Linux/AnalogIn_Navio.cpp → libraries/AP_HAL_Linux/AnalogIn_ADS1115.cpp

@ -2,10 +2,10 @@
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#include "AnalogIn_Navio.h" #include "AnalogIn_ADS1115.h"
#define NAVIO_ANALOGIN_DEBUG 0 #define ADS1115_ANALOGIN_DEBUG 0
#if NAVIO_ANALOGIN_DEBUG #if ADS1115_ANALOGIN_DEBUG
#include <cstdio> #include <cstdio>
#define debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) #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) #define error(fmt, args ...) do {fprintf(stderr,"%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
@ -14,13 +14,13 @@
#define error(fmt, args ...) #define error(fmt, args ...)
#endif #endif
NavioAnalogSource::NavioAnalogSource(int16_t pin): ADS1115AnalogSource::ADS1115AnalogSource(int16_t pin):
_pin(pin), _pin(pin),
_value(0.0f) _value(0.0f)
{ {
} }
void NavioAnalogSource::set_pin(uint8_t pin) void ADS1115AnalogSource::set_pin(uint8_t pin)
{ {
if (_pin == pin) { if (_pin == pin) {
return; return;
@ -28,44 +28,44 @@ void NavioAnalogSource::set_pin(uint8_t pin)
_pin = pin; _pin = pin;
} }
float NavioAnalogSource::read_average() float ADS1115AnalogSource::read_average()
{ {
return read_latest(); return read_latest();
} }
float NavioAnalogSource::read_latest() float ADS1115AnalogSource::read_latest()
{ {
return _value; return _value;
} }
float NavioAnalogSource::voltage_average() float ADS1115AnalogSource::voltage_average()
{ {
return _value; return _value;
} }
float NavioAnalogSource::voltage_latest() float ADS1115AnalogSource::voltage_latest()
{ {
return _value; return _value;
} }
float NavioAnalogSource::voltage_average_ratiometric() float ADS1115AnalogSource::voltage_average_ratiometric()
{ {
return _value; return _value;
} }
extern const AP_HAL::HAL& hal; extern const AP_HAL::HAL& hal;
NavioAnalogIn::NavioAnalogIn() ADS1115AnalogIn::ADS1115AnalogIn()
{ {
_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* NavioAnalogIn::channel(int16_t pin) AP_HAL::AnalogSource* ADS1115AnalogIn::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 NavioAnalogSource(pin); _channels[j] = new ADS1115AnalogSource(pin);
return _channels[j]; return _channels[j];
} }
} }
@ -74,27 +74,27 @@ AP_HAL::AnalogSource* NavioAnalogIn::channel(int16_t pin)
return NULL; return NULL;
} }
void NavioAnalogIn::init(void* implspecific) void ADS1115AnalogIn::init(void* implspecific)
{ {
_adc->init(); _adc->init();
hal.scheduler->suspend_timer_procs(); hal.scheduler->suspend_timer_procs();
hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&NavioAnalogIn::_update, void)); hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&ADS1115AnalogIn::_update, void));
hal.scheduler->resume_timer_procs(); hal.scheduler->resume_timer_procs();
} }
void NavioAnalogIn::_update() void ADS1115AnalogIn::_update()
{ {
if (hal.scheduler->micros() - _last_update_timestamp < 100000) { if (hal.scheduler->micros() - _last_update_timestamp < 100000) {
return; return;
} }
adc_report_s reports[NAVIO_ADC_MAX_CHANNELS]; adc_report_s reports[ADS1115_ADC_MAX_CHANNELS];
size_t rc = _adc->read(reports, 6); size_t rc = _adc->read(reports, 6);
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++) {
NavioAnalogSource *source = _channels[j]; ADS1115AnalogSource *source = _channels[j];
#if 0 #if 0
if (source != NULL) { if (source != NULL) {

18
libraries/AP_HAL_Linux/AnalogIn_Navio.h → libraries/AP_HAL_Linux/AnalogIn_ADS1115.h

@ -1,15 +1,15 @@
#ifndef __NavioAnalogIn_H__ #ifndef __ADS1115AnalogIn_H__
#define __NavioAnalogIn_H__ #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 NAVIO_ADC_MAX_CHANNELS 6 #define ADS1115_ADC_MAX_CHANNELS 6
class NavioAnalogSource: public AP_HAL::AnalogSource { class ADS1115AnalogSource: public AP_HAL::AnalogSource {
public: public:
friend class NavioAnalogIn; friend class ADS1115AnalogIn;
NavioAnalogSource(int16_t pin); ADS1115AnalogSource(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,9 +23,9 @@ private:
float _value; float _value;
}; };
class NavioAnalogIn: public AP_HAL::AnalogIn { class ADS1115AnalogIn: public AP_HAL::AnalogIn {
public: public:
NavioAnalogIn(); ADS1115AnalogIn();
void init(void* implspecific); void init(void* implspecific);
AP_HAL::AnalogSource* channel(int16_t n); AP_HAL::AnalogSource* channel(int16_t n);
@ -36,7 +36,7 @@ public:
} }
private: private:
AP_ADC_ADS1115 *_adc; AP_ADC_ADS1115 *_adc;
NavioAnalogSource *_channels[NAVIO_ADC_MAX_CHANNELS]; ADS1115AnalogSource *_channels[ADS1115_ADC_MAX_CHANNELS];
uint8_t _channels_number; uint8_t _channels_number;
Loading…
Cancel
Save