HAL_ChibiOS: improved auto-config of ADC pins
ADC config now based on pin_scaling array
This commit is contained in:
Andrew Tridgell
parent
1cc4b92b76
commit
b62773bbad
@ -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) :
|
||||
_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()
|
||||
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() :
|
||||
_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()
|
||||
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)
|
||||
}
|
||||
_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);
|
||||
}
|
||||
|
||||
@ -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:
|
||||
|
||||
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:
|
||||
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;
|
||||
};
|
||||
|
||||
Loading…
Reference in New Issue
Block a user