From 57e87b9820382a19a87336cae7226e9d9a618f08 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 21 Aug 2021 10:19:16 +1000 Subject: [PATCH] HAL_ChibiOS: measure MCU temperature and ref voltage on H7 this uses ADC3 to measure the internal MCU temperature and the reference voltage. This uses a currently unused BDMA channel --- libraries/AP_HAL_ChibiOS/AnalogIn.cpp | 152 ++++++++++++++++-- libraries/AP_HAL_ChibiOS/AnalogIn.h | 27 +++- .../hwdef/common/stm32h7_mcuconf.h | 2 +- .../hwdef/scripts/STM32H743xx.py | 2 +- .../hwdef/scripts/chibios_hwdef.py | 4 + .../hwdef/scripts/dma_resolver.py | 16 +- 6 files changed, 181 insertions(+), 22 deletions(-) diff --git a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp index 6a1d10ec36..c44b2b4a70 100644 --- a/libraries/AP_HAL_ChibiOS/AnalogIn.cpp +++ b/libraries/AP_HAL_ChibiOS/AnalogIn.cpp @@ -273,6 +273,10 @@ void AnalogIn::init() #endif } adcStartConversion(&ADCD1, &adcgrpcfg, samples, ADC_DMA_BUF_DEPTH); + +#if HAL_WITH_MCU_MONITORING + setup_adc3(); +#endif } /* @@ -289,6 +293,119 @@ void AnalogIn::read_adc(uint32_t *val) chSysUnlock(); } + +#if HAL_WITH_MCU_MONITORING +/* + on H7 we can support monitoring MCU temperature and voltage using ADC3 + */ +#define ADC3_GRP1_NUM_CHANNELS 3 + +// internal ADC channels (from H7 reference manual) +#define ADC3_VSENSE_CHAN 18 +#define ADC3_VREFINT_CHAN 19 +#define ADC3_VBAT4_CHAN 17 + +// samples filled in by ADC DMA engine +adcsample_t *AnalogIn::samples_adc3; +uint32_t AnalogIn::sample_adc3_sum[ADC3_GRP1_NUM_CHANNELS]; +// we also keep min and max so we can report the range of voltages +// seen, to give an idea of supply stability +uint16_t AnalogIn::sample_adc3_max[ADC3_GRP1_NUM_CHANNELS]; +uint16_t AnalogIn::sample_adc3_min[ADC3_GRP1_NUM_CHANNELS]; +uint32_t AnalogIn::sample_adc3_count; + +/* + callback from ADC3 driver when sample buffer is filled + */ +void AnalogIn::adc3callback(ADCDriver *adcp) +{ + const adcsample_t *buffer = samples_adc3; + + stm32_cacheBufferInvalidate(buffer, sizeof(adcsample_t)*ADC_DMA_BUF_DEPTH*ADC3_GRP1_NUM_CHANNELS); + for (uint8_t i = 0; i < ADC_DMA_BUF_DEPTH; i++) { + for (uint8_t j = 0; j < ADC3_GRP1_NUM_CHANNELS; j++) { + const uint16_t v = *buffer++; + sample_adc3_sum[j] += v; + if (sample_adc3_min[j] == 0 || + sample_adc3_min[j] > v) { + sample_adc3_min[j] = v; + } + if (sample_adc3_max[j] == 0 || + sample_adc3_max[j] < v) { + sample_adc3_max[j] = v; + } + } + } + sample_adc3_count += ADC_DMA_BUF_DEPTH; +} + +/* + setup ADC3 for internal temperature and voltage monitoring + */ +void AnalogIn::setup_adc3(void) +{ + samples_adc3 = (adcsample_t *)hal.util->malloc_type(sizeof(adcsample_t)*ADC_DMA_BUF_DEPTH*ADC3_GRP1_NUM_CHANNELS, AP_HAL::Util::MEM_DMA_SAFE); + + adcStart(&ADCD3, NULL); + + adcSTM32EnableVREF(&ADCD3); + adcSTM32EnableTS(&ADCD3); + adcSTM32EnableVBAT(&ADCD3); + + memset(&adc3grpcfg, 0, sizeof(adc3grpcfg)); + adc3grpcfg.circular = true; + adc3grpcfg.num_channels = ADC3_GRP1_NUM_CHANNELS; + adc3grpcfg.end_cb = adc3callback; +#if defined(ADC_CFGR_RES_16BITS) + // use 16 bit resolution + adc3grpcfg.cfgr = ADC_CFGR_CONT | ADC_CFGR_RES_16BITS; +#elif defined(ADC_CFGR_RES_12BITS) + // use 12 bit resolution + adc3grpcfg.cfgr = ADC_CFGR_CONT | ADC_CFGR_RES_12BITS; +#else + // use 12 bit resolution with ADCv1 or ADCv2 + adc3grpcfg.sqr1 = ADC_SQR1_NUM_CH(ADC3_GRP1_NUM_CHANNELS); + adc3grpcfg.cr2 = ADC_CR2_SWSTART; +#endif + + const uint8_t channels[ADC3_GRP1_NUM_CHANNELS] = { ADC3_VBAT4_CHAN, ADC3_VSENSE_CHAN, ADC3_VREFINT_CHAN }; + + for (uint8_t i=0; i 5000 && count++ == 10) { - count = 0; - uint16_t adc[6] {}; - uint8_t n = ADC_GRP1_NUM_CHANNELS; - if (n > 6) { - n = 6; - } - for (uint8_t i=0; i < n; 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]); +#if HAL_WITH_MCU_MONITORING + // 20Hz temperature and ref voltage + static uint32_t last_mcu_temp_us; + if (now - last_mcu_temp_us > 50000 && + hal.scheduler->is_system_initialized()) { + last_mcu_temp_us = now; + + uint32_t buf_adc3[ADC3_GRP1_NUM_CHANNELS]; + uint16_t min_adc3[ADC3_GRP1_NUM_CHANNELS]; + uint16_t max_adc3[ADC3_GRP1_NUM_CHANNELS]; + + read_adc3(buf_adc3, min_adc3, max_adc3); + + // factory calibration values + const float TS_CAL1 = *(const volatile uint16_t *)0x1FF1E820; + const float TS_CAL2 = *(const volatile uint16_t *)0x1FF1E840; + const float VREFINT_CAL = *(const volatile uint16_t *)0x1FF1E860; + + _mcu_temperature = ((110 - 30) / (TS_CAL2 - TS_CAL1)) * (float(buf_adc3[1]) - TS_CAL1) + 30; + _mcu_voltage = 3.3 * VREFINT_CAL / float(buf_adc3[2]+0.001); + _mcu_voltage_min = 3.3 * VREFINT_CAL / float(min_adc3[2]+0.001); + _mcu_voltage_max = 3.3 * VREFINT_CAL / float(max_adc3[2]+0.001); } #endif } diff --git a/libraries/AP_HAL_ChibiOS/AnalogIn.h b/libraries/AP_HAL_ChibiOS/AnalogIn.h index 29a58137be..cedfbf32f7 100644 --- a/libraries/AP_HAL_ChibiOS/AnalogIn.h +++ b/libraries/AP_HAL_ChibiOS/AnalogIn.h @@ -62,11 +62,18 @@ public: float servorail_voltage(void) override { return _servorail_voltage; } uint16_t power_status_flags(void) override { return _power_flags; } uint16_t accumulated_power_status_flags(void) const override { return _accumulated_power_flags; } - static void adccallback(ADCDriver *adcp); + +#if HAL_WITH_MCU_MONITORING + float mcu_temperature(void) override { return _mcu_temperature; } + float mcu_voltage(void) override { return _mcu_voltage; } + float mcu_voltage_max(void) override { return _mcu_voltage_max; } + float mcu_voltage_min(void) override { return _mcu_voltage_min; } +#endif private: void read_adc(uint32_t *val); void update_power_flags(void); + static void adccallback(ADCDriver *adcp); ChibiOS::AnalogSource* _channels[ANALOG_MAX_CHANNELS]; @@ -88,7 +95,25 @@ private: static adcsample_t *samples; static uint32_t sample_sum[]; static uint32_t sample_count; + HAL_Semaphore _semaphore; + +#if HAL_WITH_MCU_MONITORING + // use ADC3 for MCU temperature and voltage monitoring + void setup_adc3(); + void read_adc3(uint32_t *val, uint16_t *min, uint16_t *max); + ADCConversionGroup adc3grpcfg; + static void adc3callback(ADCDriver *adcp); + static adcsample_t *samples_adc3; + static uint32_t sample_adc3_sum[]; + static uint16_t sample_adc3_max[]; + static uint16_t sample_adc3_min[]; + static uint32_t sample_adc3_count; + float _mcu_temperature; + float _mcu_voltage; + float _mcu_voltage_min; + float _mcu_voltage_max; +#endif }; #endif // HAL_USE_ADC diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h index 3c9b08166e..f84dc9a76d 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32h7_mcuconf.h @@ -349,7 +349,7 @@ #define STM32_ADC_DUAL_MODE FALSE #define STM32_ADC_COMPACT_SAMPLES FALSE #define STM32_ADC_USE_ADC12 TRUE -#define STM32_ADC_USE_ADC3 FALSE +#define STM32_ADC_USE_ADC3 TRUE #define STM32_ADC_ADC12_DMA_PRIORITY 2 #define STM32_ADC_ADC3_DMA_PRIORITY 2 #define STM32_ADC_ADC12_IRQ_PRIORITY 5 diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32H743xx.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32H743xx.py index 1eacf9ec89..ec0465f1c3 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32H743xx.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/STM32H743xx.py @@ -14,7 +14,7 @@ mcu = { # DMA peripheral capabilities: # - can't use ITCM or DTCM for any DMA # - SPI1 to SPI5 can use AXI SRAM, SRAM1 to SRAM3 and SRAM4 for DMA - # - SPI6, I2C4 and ADC3 can use SRAM4 on BDMA (I didn't actually test ADC3) + # - SPI6, I2C4 and ADC3 can use SRAM4 on BDMA # - UARTS can use AXI SRAM, SRAM1 to SRAM3 and SRAM4 for DMA # - I2C1, I2C2 and I2C3 can use AXI SRAM, SRAM1 to SRAM3 and SRAM4 with DMA # - timers can use AXI SRAM, SRAM1 to SRAM3 and SRAM4 with DMA diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index e7aef09576..92aac71b85 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -1896,6 +1896,10 @@ def write_hwdef_header(outfilename): write_peripheral_enable(f) + if mcu_series.startswith("STM32H7"): + # add in ADC3 on H7 to get MCU temperature and reference voltage + periph_list.append('ADC3') + dma_unassigned, ordered_timers = dma_resolver.write_dma_header(f, periph_list, mcu_type, dma_exclude=get_dma_exclude(periph_list), dma_priority=get_config('DMA_PRIORITY', default='TIM* SPI*', spaces=True), diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py index 02606a3f9d..a2ad7ef82c 100755 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/dma_resolver.py @@ -233,9 +233,9 @@ def generate_DMAMUX_map(peripheral_list, noshare_list, dma_exclude, stream_ofs): map1 = generate_DMAMUX_map_mask(dmamux1_peripherals, 0xFFFF, noshare_list, dma_exclude, stream_ofs) # there are 8 BDMA channels, but an issue has been found where if I2C4 and SPI6 # use neighboring channels then we sometimes lose a BDMA completion interrupt. To - # avoid this we set the BDMA available mask to 0x33, which forces the channels not to be + # avoid this we set the BDMA available mask to 0x54, which forces the channels not to be # adjacent. This issue was found on a CUAV-X7, with H743 RevV. - map2 = generate_DMAMUX_map_mask(dmamux2_peripherals, 0x55, noshare_list, dma_exclude, stream_ofs) + map2 = generate_DMAMUX_map_mask(dmamux2_peripherals, 0x54, noshare_list, dma_exclude, stream_ofs) # translate entries from map2 to "DMA controller 3", which is used for BDMA for p in map2.keys(): streams = [] @@ -448,10 +448,14 @@ def write_dma_header(f, peripheral_list, mcu_type, dma_exclude=[], dma_controller = curr_dict[key][0] if dma_controller == 3: # for BDMA we use 3 in the resolver - dma_controller = 1 - f.write("#define %-30s STM32_DMA_STREAM_ID(%u, %u)%s\n" % - (chibios_dma_define_name(key)+'STREAM', dma_controller, - curr_dict[key][1], shared)) + f.write("#define %-30s %u%s\n" % + (chibios_dma_define_name(key)+'STREAM', + curr_dict[key][1], shared)) + continue + else: + f.write("#define %-30s STM32_DMA_STREAM_ID(%u, %u)%s\n" % + (chibios_dma_define_name(key)+'STREAM', dma_controller, + curr_dict[key][1], shared)) if have_DMAMUX and "_UP" in key: # share the dma with rest of the _CH ports for ch in range(1,5):