@ -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 ] ) ;
}