Browse Source

HAL_ChibiOS: fixed ADC error on revXY STM32H7 boards

with the new ChibiOS revision we were configuring for newer revisions
of the H7, which meant that we changed the ADC config. This broke ADC
readings on revX and revY H7 MCUs.

This PR fixes it in two ways:

 1) change ChibiOS config to assume XY config chips

 2) use 16 bit ADC for H7, which means the chip rev doesn't matter,
    and also gives us 16x the resolution for ADC readinga, so we can
    read smaller voltage and current values
zr-v5.1
Andrew Tridgell 4 years ago
parent
commit
ca948171fe
  1. 22
      libraries/AP_HAL_ChibiOS/AnalogIn.cpp
  2. 3
      libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h

22
libraries/AP_HAL_ChibiOS/AnalogIn.cpp

@ -64,6 +64,14 @@ const AnalogIn::pin_info AnalogIn::pin_config[] = HAL_ANALOG_PINS; @@ -64,6 +64,14 @@ const AnalogIn::pin_info AnalogIn::pin_config[] = HAL_ANALOG_PINS;
#define ADC_GRP1_NUM_CHANNELS ARRAY_SIZE(AnalogIn::pin_config)
#if defined(STM32H7) || defined(STM32F3) || defined(STM32G4)
// on H7 we use 16 bit ADC transfers, giving us more resolution. We
// need to scale by 1/16 to match the 12 bit scale factors in hwdef.dat
#define ADC_BOARD_SCALING (1.0/16)
#else
#define ADC_BOARD_SCALING 1
#endif
// samples filled in by ADC DMA engine
adcsample_t *AnalogIn::samples;
uint32_t AnalogIn::sample_sum[ADC_GRP1_NUM_CHANNELS];
@ -210,10 +218,10 @@ void AnalogIn::init() @@ -210,10 +218,10 @@ void AnalogIn::init()
adcgrpcfg.num_channels = ADC_GRP1_NUM_CHANNELS;
adcgrpcfg.end_cb = adccallback;
#if defined(STM32H7) || defined(STM32F3) || defined(STM32G4)
// use 12 bits resolution to keep scaling factors the same as other boards.
// todo: enable oversampling in cfgr2 ?
adcgrpcfg.cfgr = ADC_CFGR_CONT | ADC_CFGR_RES_12BITS;
// use 16 bit resolution
adcgrpcfg.cfgr = ADC_CFGR_CONT | ADC_CFGR_RES_16BITS;
#else
// use 12 bit resolution
adcgrpcfg.sqr1 = ADC_SQR1_NUM_CH(ADC_GRP1_NUM_CHANNELS);
adcgrpcfg.cr2 = ADC_CR2_SWSTART;
#endif
@ -305,12 +313,12 @@ void AnalogIn::_timer_tick(void) @@ -305,12 +313,12 @@ void AnalogIn::_timer_tick(void)
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_config[i].scaling;
_board_voltage = buf_adc[i] * pin_config[i].scaling * ADC_BOARD_SCALING;
}
#endif
#ifdef FMU_SERVORAIL_ADC_CHAN
if (pin_config[i].channel == FMU_SERVORAIL_ADC_CHAN) {
_servorail_voltage = buf_adc[i] * pin_config[i].scaling;
_servorail_voltage = buf_adc[i] * pin_config[i].scaling * ADC_BOARD_SCALING;
}
#endif
}
@ -330,7 +338,7 @@ void AnalogIn::_timer_tick(void) @@ -330,7 +338,7 @@ void AnalogIn::_timer_tick(void)
if (c != nullptr) {
if (pin_config[i].channel == c->_pin) {
// add a value
c->_add_value(buf_adc[i], _board_voltage);
c->_add_value(buf_adc[i] * ADC_BOARD_SCALING, _board_voltage);
} else if (c->_pin == ANALOG_SERVO_VRSSI_PIN) {
c->_add_value(_rssi_voltage / VOLTAGE_SCALING, 0);
}
@ -348,7 +356,7 @@ void AnalogIn::_timer_tick(void) @@ -348,7 +356,7 @@ void AnalogIn::_timer_tick(void)
n = 6;
}
for (uint8_t i=0; i < n; i++) {
adc[i] = buf_adc[i];
adc[i] = buf_adc[i] * ADC_BOARD_SCALING;
}
mavlink_msg_ap_adc_send(MAVLINK_COMM_0, adc[0], adc[1], adc[2], adc[3], adc[4], adc[5]);
}

3
libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h

@ -18,6 +18,9 @@ @@ -18,6 +18,9 @@
*/
#pragma once
// we want to cope with both revision XY chips and newer chips
#define STM32_ENFORCE_H7_REV_XY
#ifndef STM32_LSECLK
#define STM32_LSECLK 32768U
#endif

Loading…
Cancel
Save