zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

HAL_ChibiOS: improved auto-config of ADC pins 

ADC config now based on pin_scaling array
master
Andrew Tridgell 7 years ago
parent
1cc4b92b76
commit
b62773bbad
2 changed files with 80 additions and 53 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 119
      libraries/AP_HAL_ChibiOS/AnalogIn.cpp
  2. 14
      libraries/AP_HAL_ChibiOS/AnalogIn.h

119
libraries/AP_HAL_ChibiOS/AnalogIn.cpp
Unescape View File

@ -36,37 +36,37 @@ extern AP_IOMCU iomcu; @@ -36,37 +36,37 @@ extern AP_IOMCU iomcu;
extern const AP_HAL::HAL& hal;
#define ADC_GRP1_NUM_CHANNELS 3
#define ADC_GRP1_BUF_DEPTH 8
static adcsample_t samples[ADC_GRP1_NUM_CHANNELS * ADC_GRP1_BUF_DEPTH] = {0};
static float avg_samples[ADC_GRP1_NUM_CHANNELS];
using namespace ChibiOS;
// special pin numbers
#define ANALOG_VCC_5V_PIN 4
// pin number for VCC rail
#define ANALOG_VCC_5V_PIN 4
/*
scaling table between ADC count and actual input voltage, to account
for voltage dividers on the board.
*/
static const struct {
uint8_t channel;
float scaling;
} pin_scaling[] = {
const ChibiAnalogIn::pin_info ChibiAnalogIn::pin_config[] = {
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412
{ ANALOG_VCC_5V_PIN, 0.007734 }, // VCC 5V rail sense
#else
{ ANALOG_VCC_5V_PIN, 6.6f/4096 }, // VCC 5V rail sense
#endif
{ 2, 3.3f/4096 }, // 3DR Brick voltage, usually 10.1:1
// scaled from battery voltage
{ 3, 3.3f/4096 }, // 3DR Brick current, usually 17:1 scaled
// for APM_PER_VOLT
{ 13, 3.3f/4096 }, // AUX ADC pin 4
{ 14, 3.3f/4096 }, // AUX ADC pin 3
{ 15, 6.6f/4096 }, // analog airspeed sensor, 2:1 scaling
{ 2, VOLTAGE_SCALING }, // 3DR Brick voltage
{ 3, VOLTAGE_SCALING }, // 3DR Brick current
{ 13, VOLTAGE_SCALING }, // AUX ADC pin 4
{ 14, VOLTAGE_SCALING }, // AUX ADC pin 3
{ 15, VOLTAGE_SCALING*2 }, // analog airspeed sensor, 2:1 scaling
};
using namespace ChibiOS;
#define ADC_GRP1_NUM_CHANNELS ARRAY_SIZE(ChibiAnalogIn::pin_config)
// samples filled in by ADC DMA engine
adcsample_t ChibiAnalogIn::samples[ADC_DMA_BUF_DEPTH*ADC_GRP1_NUM_CHANNELS];
uint32_t ChibiAnalogIn::sample_sum[ADC_GRP1_NUM_CHANNELS];
uint32_t ChibiAnalogIn::sample_count;
// special pin numbers
#define ANALOG_VCC_5V_PIN 4
ChibiAnalogSource::ChibiAnalogSource(int16_t pin, float initial_value) :
_pin(pin),
@ -77,12 +77,6 @@ ChibiAnalogSource::ChibiAnalogSource(int16_t pin, float initial_value) : @@ -77,12 +77,6 @@ ChibiAnalogSource::ChibiAnalogSource(int16_t pin, float initial_value) :
_sum_value(0),
_sum_ratiometric(0)
{
/*#ifdef PX4_ANALOG_VCC_5V_PIN
if (_pin == ANALOG_INPUT_BOARD_VCC) {
_pin = PX4_ANALOG_VCC_5V_PIN;
}
#endif
*/
}
@ -110,10 +104,9 @@ float ChibiAnalogSource::read_latest() @@ -110,10 +104,9 @@ float ChibiAnalogSource::read_latest()
float ChibiAnalogSource::_pin_scaler(void)
{
float scaling = VOLTAGE_SCALING;
uint8_t num_scalings = ARRAY_SIZE(pin_scaling) - 1;
for (uint8_t i=0; i<num_scalings; i++) {
if (pin_scaling[i].channel == _pin) {
scaling = pin_scaling[i].scaling;
for (uint8_t i=0; i<ADC_GRP1_NUM_CHANNELS; i++) {
if (ChibiAnalogIn::pin_config[i].channel == _pin) {
scaling = ChibiAnalogIn::pin_config[i].scaling;
break;
}
}
@ -188,30 +181,27 @@ ChibiAnalogIn::ChibiAnalogIn() : @@ -188,30 +181,27 @@ ChibiAnalogIn::ChibiAnalogIn() :
_servorail_voltage(0),
_power_flags(0)
{
memset(samples, 0, sizeof(samples));
for (uint8_t i = 0; i < ADC_GRP1_NUM_CHANNELS; i++) {
avg_samples[i] = 0.0f;
}
}
/*
callback from ADC driver when sample buffer is filled
*/
void ChibiAnalogIn::adccallback(ADCDriver *adcp, adcsample_t *buffer, size_t n)
{
if (buffer != samples) {
if (buffer != &samples[0]) {
return;
}
for (uint8_t i = 0; i < ADC_GRP1_NUM_CHANNELS; i++) {
avg_samples[i] = 0.0f;
}
for (uint8_t i = 0; i < ADC_GRP1_BUF_DEPTH; i++) {
for (uint8_t i = 0; i < ADC_DMA_BUF_DEPTH; i++) {
for (uint8_t j = 0; j < ADC_GRP1_NUM_CHANNELS; j++) {
avg_samples[j] += samples[(i*ADC_GRP1_NUM_CHANNELS)+j];
sample_sum[j] += *buffer++;
}
}
for (uint8_t i = 0; i < ADC_GRP1_NUM_CHANNELS; i++) {
avg_samples[i] /= ADC_GRP1_BUF_DEPTH;
}
sample_count += ADC_DMA_BUF_DEPTH;
}
/*
setup adc peripheral to capture samples with DMA into a buffer
*/
void ChibiAnalogIn::init()
{
adcStart(&ADCD1, NULL);
@ -220,20 +210,42 @@ void ChibiAnalogIn::init() @@ -220,20 +210,42 @@ void ChibiAnalogIn::init()
adcgrpcfg.num_channels = ADC_GRP1_NUM_CHANNELS;
adcgrpcfg.end_cb = adccallback;
adcgrpcfg.cr2 = ADC_CR2_SWSTART;
adcgrpcfg.smpr2 = ADC_SMPR2_SMP_AN2(ADC_SAMPLE_480) | ADC_SMPR2_SMP_AN3(ADC_SAMPLE_480) |
ADC_SMPR2_SMP_AN4(ADC_SAMPLE_480);
adcgrpcfg.sqr1 = ADC_SQR1_NUM_CH(ADC_GRP1_NUM_CHANNELS),
adcgrpcfg.sqr3 = ADC_SQR3_SQ1_N(ADC_CHANNEL_IN4) |ADC_SQR3_SQ2_N(ADC_CHANNEL_IN2) |
ADC_SQR3_SQ3_N(ADC_CHANNEL_IN3);
adcStartConversion(&ADCD1, &adcgrpcfg, &samples[0], ADC_GRP1_BUF_DEPTH);
adcgrpcfg.sqr1 = ADC_SQR1_NUM_CH(ADC_GRP1_NUM_CHANNELS);
for (uint8_t i=0; i<ADC_GRP1_NUM_CHANNELS; i++) {
uint8_t chan = pin_config[i].channel;
// setup cycles per sample for the channel
if (chan < 10) {
adcgrpcfg.smpr2 |= ADC_SAMPLE_480 << (3*chan);
} else {
adcgrpcfg.smpr1 |= ADC_SAMPLE_480 << (3*(chan-10));
}
// setup channel sequence
if (i < 6) {
adcgrpcfg.sqr3 |= chan << (5*i);
} else if (i < 12) {
adcgrpcfg.sqr2 |= chan << (5*(i-6));
} else {
adcgrpcfg.sqr1 |= chan << (5*(i-12));
}
}
adcStartConversion(&ADCD1, &adcgrpcfg, &samples[0], ADC_DMA_BUF_DEPTH);
}
/*
calculate average sample since last read for all channels
*/
void ChibiAnalogIn::read_adc(uint32_t *val)
{
chSysLock();
for (uint8_t i = 0; i < ADC_GRP1_NUM_CHANNELS; i++) {
val[i] = avg_samples[i];
val[i] = sample_sum[i] / sample_count;
}
memset(sample_sum, 0, sizeof(sample_sum));
sample_count = 0;
chSysUnlock();
}
/*
called at 1kHz
*/
@ -247,25 +259,26 @@ void ChibiAnalogIn::_timer_tick(void) @@ -247,25 +259,26 @@ void ChibiAnalogIn::_timer_tick(void)
}
_last_run = now;
uint32_t buf_adc[ADC_GRP1_NUM_CHANNELS] = {0};
uint32_t buf_adc[ADC_GRP1_NUM_CHANNELS];
/* read all channels available */
read_adc(buf_adc);
// match the incoming channels to the currently active pins
for (uint8_t i=0; i < ADC_GRP1_NUM_CHANNELS; i++) {
if (pin_scaling[i].channel == ANALOG_VCC_5V_PIN) {
if (pin_config[i].channel == ANALOG_VCC_5V_PIN) {
// record the Vcc value for later use in
// voltage_average_ratiometric()
_board_voltage = buf_adc[i] * pin_scaling[i].scaling;
_board_voltage = buf_adc[i] * pin_config[i].scaling;
}
}
for (uint8_t i=0; i<ADC_GRP1_NUM_CHANNELS; i++) {
Debug("chan %u value=%u\n",
(unsigned)pin_scaling[i].channel,
(unsigned)pin_config[i].channel,
(unsigned)buf_adc[i]);
for (uint8_t j=0; j < ADC_GRP1_NUM_CHANNELS; j++) {
ChibiOS::ChibiAnalogSource *c = _channels[j];
if (c != nullptr && pin_scaling[i].channel == c->_pin) {
if (c != nullptr && pin_config[i].channel == c->_pin) {
// add a value
c->_add_value(buf_adc[i], _board_voltage);
}

14
libraries/AP_HAL_ChibiOS/AnalogIn.h
Unescape View File

@ -21,6 +21,8 @@ @@ -21,6 +21,8 @@
#define ANALOG_MAX_CHANNELS 16
// number of samples on each channel to gather on each DMA callback
#define ADC_DMA_BUF_DEPTH 8
class ChibiOS::ChibiAnalogSource : public AP_HAL::AnalogSource {
public:
@ -50,6 +52,8 @@ private: @@ -50,6 +52,8 @@ private:
class ChibiOS::ChibiAnalogIn : public AP_HAL::AnalogIn {
public:
friend class ChibiAnalogSource;
ChibiAnalogIn();
void init() override;
AP_HAL::AnalogSource* channel(int16_t pin) override;
@ -72,4 +76,14 @@ private: @@ -72,4 +76,14 @@ private:
float _servorail_voltage;
uint16_t _power_flags;
ADCConversionGroup adcgrpcfg;
struct pin_info {
uint8_t channel;
float scaling;
};
static const pin_info pin_config[];
static adcsample_t samples[];
static uint32_t sample_sum[];
static uint32_t sample_count;
};

Write Preview
Loading…
Cancel
Save