diff --git a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp index 208c964e62..bb1b31e2d5 100644 --- a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp +++ b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp @@ -403,9 +403,13 @@ void AnalogIn::init() #else static_assert(sizeof(uint16_t) == sizeof(adcsample_t), "adcsample_t must be uint16_t"); #endif - for (uint8_t i=0; i_pin) && (c->_pin != ANALOG_INPUT_NONE)) { + // add a value + 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); + } + } + } + } +} + /* called at 1kHz */ @@ -643,47 +693,16 @@ void AnalogIn::_timer_tick(void) _rssi_voltage = iomcu.get_vrssi_adc_count() * (VOLTAGE_SCALING * HAL_IOMCU_VRSSI_SCALAR); #endif - for (uint8_t index=0; index < HAL_NUM_ANALOG_INPUTS; index++) { - const uint8_t num_grp_channels = get_num_grp_channels(index); - uint32_t buf_adc[num_grp_channels]; - - /* read all channels available on index ADC*/ - read_adc(index, buf_adc); - - // match the incoming channels to the currently active pins - for (uint8_t i=0; i < num_grp_channels; i++) { - #ifdef ANALOG_VCC_5V_PIN - if (get_analog_pin(index, i) == ANALOG_VCC_5V_PIN) { - // record the Vcc value for later use in - // voltage_average_ratiometric() - _board_voltage = buf_adc[i] * get_pin_scaling(index, i) * ADC_BOARD_SCALING; - } - #endif - #ifdef FMU_SERVORAIL_ADC_PIN - if (get_analog_pin(index, i) == FMU_SERVORAIL_ADC_PIN) { - _servorail_voltage = buf_adc[i] * get_pin_scaling(index, i) * ADC_BOARD_SCALING; - } - #endif - } - - for (uint8_t i=0; i_pin) && (c->_pin != ANALOG_INPUT_NONE)) { - // add a value - 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); - } - } - } - } - } + /* + update each of our ADCs + */ + timer_tick_adc(0); +#if defined(HAL_ANALOG2_PINS) + timer_tick_adc(1); +#endif +#if defined(HAL_ANALOG3_PINS) + timer_tick_adc(2); +#endif #if HAL_WITH_IO_MCU _servorail_voltage = iomcu.get_vservo_adc_count() * (VOLTAGE_SCALING * HAL_IOMCU_VSERVO_SCALAR); diff --git a/libraries/AP_HAL_ChibiOS/AnalogIn.h b/libraries/AP_HAL_ChibiOS/AnalogIn.h index 952578289c..8c134a0d32 100644 --- a/libraries/AP_HAL_ChibiOS/AnalogIn.h +++ b/libraries/AP_HAL_ChibiOS/AnalogIn.h @@ -73,6 +73,7 @@ public: void init() override; AP_HAL::AnalogSource* channel(int16_t pin) override; void _timer_tick(void); + void timer_tick_adc(uint8_t index); float board_voltage(void) override { return _board_voltage; } float servorail_voltage(void) override { return _servorail_voltage; } uint16_t power_status_flags(void) override { return _power_flags; }