From 14dcad6bbc5288b1205a201139218e37e9ff6d7a Mon Sep 17 00:00:00 2001 From: Christopher Pepper Date: Sun, 7 Jan 2018 22:15:20 +0000 Subject: [PATCH] LPC1768: Increase ADC median filter from 3 values to 23 Clarify the HAL_adc_get_result method to make sure correct values enter the filters HAL: Fix the PID control loop for non-AVR platforms --- Marlin/src/HAL/HAL_LPC1768/HAL.cpp | 150 ++++++++++++++---- .../HAL/HAL_LPC1768/HAL_LCD_pin_routines.c | 2 +- Marlin/src/module/temperature.h | 2 +- 3 files changed, 117 insertions(+), 37 deletions(-) diff --git a/Marlin/src/HAL/HAL_LPC1768/HAL.cpp b/Marlin/src/HAL/HAL_LPC1768/HAL.cpp index 5579810346..f74fa5fe69 100644 --- a/Marlin/src/HAL/HAL_LPC1768/HAL.cpp +++ b/Marlin/src/HAL/HAL_LPC1768/HAL.cpp @@ -22,11 +22,6 @@ #include "../../inc/MarlinConfig.h" -extern "C" { - //#include - //#include -} - HalSerial usb_serial; //u8glib required fucntions @@ -112,7 +107,6 @@ void HAL_adc_enable_channel(int ch) { }; } -uint8_t active_adc = 0; void HAL_adc_start_conversion(const uint8_t ch) { if (analogInputToDigitalPin(ch) == -1) { MYSERIAL.printf("HAL: HAL_adc_start_conversion: invalid channel %d\n", ch); @@ -122,7 +116,6 @@ void HAL_adc_start_conversion(const uint8_t ch) { LPC_ADC->ADCR &= ~0xFF; // Reset SBI(LPC_ADC->ADCR, ch); // Select Channel SBI(LPC_ADC->ADCR, 24); // Start conversion - active_adc = ch; } bool HAL_adc_finished(void) { @@ -130,44 +123,131 @@ bool HAL_adc_finished(void) { } // possible config options if something similar is extended to more platforms. -#define ADC_USE_MEDIAN_FILTER // filter out erroneous readings -#define ADC_USE_LOWPASS_FILTER // filter out high frequency noise -#define ADC_LOWPASS_K_VALUE 4 // how much to smooth out noise (1:8) +#define ADC_USE_MEDIAN_FILTER // Filter out erroneous readings +#define ADC_MEDIAN_FILTER_SIZE (23) // Higher values increase step delay (phase shift), + // (ADC_MEDIAN_FILTER_SIZE + 1) / 2 sample step delay (12 samples @ 500Hz: 24ms phase shift) + // Memory usage per ADC channel (bytes): (6 * ADC_MEDIAN_FILTER_SIZE) + 16 + // 8 * ((6 * 23) + 16 ) = 1232 Bytes for 8 channels +#define ADC_USE_LOWPASS_FILTER // Filter out high frequency noise +#define ADC_LOWPASS_K_VALUE (6) // Higher values increase rise time + // Rise time sample delays for 100% signal convergence on full range step + // (1 : 13, 2 : 32, 3 : 67, 4 : 139, 5 : 281, 6 : 565, 7 : 1135, 8 : 2273) + // K = 6, 565 samples, 500Hz sample rate, 1.13s convergence on full range step + // Memory usage per ADC channel (bytes): 4 (32 Bytes for 8 channels) + + +// Sourced from https://embeddedgurus.com/stack-overflow/tag/median-filter/ struct MedianFilter { - uint16_t values[3]; - uint8_t next_val; - MedianFilter() { - next_val = 0; - values[0] = values[1] = values[2] = 0; - } - uint16_t update(uint16_t value) { - values[next_val++] = value; - next_val = next_val % 3; - return max(min(values[0], values[1]), min(max(values[0], values[1]), values[2])); //median + #define STOPPER 0 // Smaller than any datum + struct Pair { + Pair *point; // Pointers forming list linked in sorted order + uint16_t value; // Values to sort + }; + + Pair buffer[ADC_MEDIAN_FILTER_SIZE] = {}; // Buffer of nwidth pairs + Pair *datpoint = buffer; // Pointer into circular buffer of data + Pair small = {NULL, STOPPER}; // Chain stopper + Pair big = {&small, 0}; // Pointer to head (largest) of linked list. + + uint16_t update(uint16_t datum) { + Pair *successor; // Pointer to successor of replaced data item + Pair *scan; // Pointer used to scan down the sorted list + Pair *scanold; // Previous value of scan + Pair *median; // Pointer to median + uint16_t i; + + if (datum == STOPPER) { + datum = STOPPER + 1; // No stoppers allowed. + } + + if ( (++datpoint - buffer) >= ADC_MEDIAN_FILTER_SIZE) { + datpoint = buffer; // Increment and wrap data in pointer. + } + + datpoint->value = datum; // Copy in new datum + successor = datpoint->point; // Save pointer to old value's successor + median = &big; // Median initially to first in chain + scanold = NULL; // Scanold initially null. + scan = &big; // Points to pointer to first (largest) datum in chain + + // Handle chain-out of first item in chain as special case + if (scan->point == datpoint) { + scan->point = successor; + } + scanold = scan; // Save this pointer and + scan = scan->point ; // step down chain + + // Loop through the chain, normal loop exit via break. + for (i = 0 ; i < ADC_MEDIAN_FILTER_SIZE; ++i) { + // Handle odd-numbered item in chain + if (scan->point == datpoint) { + scan->point = successor; // Chain out the old datum + } + + if (scan->value < datum) { // If datum is larger than scanned value + datpoint->point = scanold->point; // Chain it in here + scanold->point = datpoint; // Mark it chained in + datum = STOPPER; + } + + // Step median pointer down chain after doing odd-numbered element + median = median->point; // Step median pointer + if (scan == &small) { + break; // Break at end of chain + } + scanold = scan; // Save this pointer and + scan = scan->point; // step down chain + + // Handle even-numbered item in chain. + if (scan->point == datpoint) { + scan->point = successor; + } + + if (scan->value < datum) { + datpoint->point = scanold->point; + scanold->point = datpoint; + datum = STOPPER; + } + + if (scan == &small) { + break; + } + + scanold = scan; + scan = scan->point; + } + return median->value; } }; -uint16_t lowpass_filter(uint16_t value) { - const uint8_t k_data_shift = ADC_LOWPASS_K_VALUE; - static uint32_t data_delay[NUM_ANALOG_INPUTS] = { 0 }; - uint32_t &active_filter = data_delay[active_adc]; - active_filter = active_filter - (active_filter >> k_data_shift) + value; - return (uint16_t)(active_filter >> k_data_shift); -} +struct LowpassFilter { + uint32_t data_delay = 0; + uint16_t update(uint16_t value) { + data_delay = data_delay - (data_delay >> ADC_LOWPASS_K_VALUE) + value; + return (uint16_t)(data_delay >> ADC_LOWPASS_K_VALUE); + } +}; uint16_t HAL_adc_get_result(void) { - uint32_t data = LPC_ADC->ADGDR; - CBI(LPC_ADC->ADCR, 24); // Stop conversion - if (data & ADC_OVERRUN) return 0; + uint32_t adgdr = LPC_ADC->ADGDR; + CBI(LPC_ADC->ADCR, 24); // Stop conversion + + if (adgdr & ADC_OVERRUN) return 0; + uint16_t data = (adgdr >> 4) & 0xFFF; // copy the 12bit data value + uint8_t adc_channel = (adgdr >> 24) & 0x7; // copy the 3bit used channel + #ifdef ADC_USE_MEDIAN_FILTER static MedianFilter median_filter[NUM_ANALOG_INPUTS]; - data = median_filter[active_adc].update((uint16_t)data); + data = median_filter[adc_channel].update(data); #endif + #ifdef ADC_USE_LOWPASS_FILTER - data = lowpass_filter((uint16_t)data); + static LowpassFilter lowpass_filter[NUM_ANALOG_INPUTS]; + data = lowpass_filter[adc_channel].update(data); #endif - return ((data >> 6) & 0x3ff); // 10bit + + return ((data >> 2) & 0x3ff); // return 10bit value as Marlin expects } #define SBIT_CNTEN 0 @@ -187,8 +267,8 @@ void HAL_pwm_init(void) { LPC_PWM1->TCR = _BV(SBIT_CNTEN) | _BV(SBIT_PWMEN); LPC_PWM1->PR = 0x0; // No prescalar LPC_PWM1->MCR = _BV(SBIT_PWMMR0R); // Reset on PWMMR0, reset TC if it matches MR0 - LPC_PWM1->MR0 = 255; /* set PWM cycle(Ton+Toff)=255) */ - LPC_PWM1->MR5 = 0; /* Set 50% Duty Cycle for the channels */ + LPC_PWM1->MR0 = 255; // set PWM cycle(Ton+Toff)=255) + LPC_PWM1->MR5 = 0; // Set 50% Duty Cycle for the channels LPC_PWM1->MR6 = 0; // Trigger the latch Enable Bits to load the new Match Values MR0, MR5, MR6 diff --git a/Marlin/src/HAL/HAL_LPC1768/HAL_LCD_pin_routines.c b/Marlin/src/HAL/HAL_LPC1768/HAL_LCD_pin_routines.c index f4e5a76b47..0146c4fb67 100644 --- a/Marlin/src/HAL/HAL_LPC1768/HAL_LCD_pin_routines.c +++ b/Marlin/src/HAL/HAL_LPC1768/HAL_LCD_pin_routines.c @@ -34,7 +34,7 @@ #include #include -#include "src/core/macros.h" +#include "../../src/core/macros.h" //#include "pinmapping.h" #define LPC_PORT_OFFSET (0x0020) diff --git a/Marlin/src/module/temperature.h b/Marlin/src/module/temperature.h index 52d49dcfa6..68b00586c9 100644 --- a/Marlin/src/module/temperature.h +++ b/Marlin/src/module/temperature.h @@ -90,7 +90,7 @@ enum ADCSensorState { #if HAS_PID_HEATING #define PID_K2 (1.0-PID_K1) - #define PID_dT ((OVERSAMPLENR * float(ACTUAL_ADC_SAMPLES)) / (F_CPU / 64.0 / 256.0)) + #define PID_dT ((OVERSAMPLENR * float(ACTUAL_ADC_SAMPLES)) / TEMP_TIMER_FREQUENCY) // Apply the scale factors to the PID values #define scalePID_i(i) ( (i) * PID_dT )