Browse Source

Merge pull request #8996 from p3p/pr_bf2_more_adc_filter

[2.0.x] PID control fix and more LPC1768 ADC filtering
pull/1/head
Scott Lahteine 7 years ago
committed by GitHub
parent
commit
d5bf9e74c4
No known key found for this signature in database GPG Key ID: 4AEE18F83AFDEB23
  1. 148
      Marlin/src/HAL/HAL_LPC1768/HAL.cpp
  2. 2
      Marlin/src/HAL/HAL_LPC1768/HAL_LCD_pin_routines.c
  3. 2
      Marlin/src/module/temperature.h

148
Marlin/src/HAL/HAL_LPC1768/HAL.cpp

@ -22,11 +22,6 @@
#include "../../inc/MarlinConfig.h" #include "../../inc/MarlinConfig.h"
extern "C" {
//#include <lpc17xx_adc.h>
//#include <lpc17xx_pinsel.h>
}
HalSerial usb_serial; HalSerial usb_serial;
// U8glib required functions // U8glib required functions
@ -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) { void HAL_adc_start_conversion(const uint8_t ch) {
if (analogInputToDigitalPin(ch) == -1) { if (analogInputToDigitalPin(ch) == -1) {
SERIAL_PRINTF("HAL: HAL_adc_start_conversion: invalid channel %d\n", ch); SERIAL_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 LPC_ADC->ADCR &= ~0xFF; // Reset
SBI(LPC_ADC->ADCR, ch); // Select Channel SBI(LPC_ADC->ADCR, ch); // Select Channel
SBI(LPC_ADC->ADCR, 24); // Start conversion SBI(LPC_ADC->ADCR, 24); // Start conversion
active_adc = ch;
} }
bool HAL_adc_finished(void) { 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. // possible config options if something similar is extended to more platforms.
#define ADC_USE_MEDIAN_FILTER // filter out erroneous readings #define ADC_USE_MEDIAN_FILTER // Filter out erroneous readings
#define ADC_USE_LOWPASS_FILTER // filter out high frequency noise #define ADC_MEDIAN_FILTER_SIZE (23) // Higher values increase step delay (phase shift),
#define ADC_LOWPASS_K_VALUE 4 // how much to smooth out noise (1:8) // (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 { struct MedianFilter {
uint16_t values[3]; #define STOPPER 0 // Smaller than any datum
uint8_t next_val; struct Pair {
MedianFilter() { Pair *point; // Pointers forming list linked in sorted order
next_val = 0; uint16_t value; // Values to sort
values[0] = values[1] = values[2] = 0; };
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;
} }
};
struct LowpassFilter {
uint32_t data_delay = 0;
uint16_t update(uint16_t value) { uint16_t update(uint16_t value) {
values[next_val++] = value; data_delay = data_delay - (data_delay >> ADC_LOWPASS_K_VALUE) + value;
next_val = next_val % 3; return (uint16_t)(data_delay >> ADC_LOWPASS_K_VALUE);
return max(min(values[0], values[1]), min(max(values[0], values[1]), values[2])); //median
} }
}; };
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);
}
uint16_t HAL_adc_get_result(void) { uint16_t HAL_adc_get_result(void) {
uint32_t data = LPC_ADC->ADGDR; uint32_t adgdr = LPC_ADC->ADGDR;
CBI(LPC_ADC->ADCR, 24); // Stop conversion CBI(LPC_ADC->ADCR, 24); // Stop conversion
if (data & ADC_OVERRUN) return 0;
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 #ifdef ADC_USE_MEDIAN_FILTER
static MedianFilter median_filter[NUM_ANALOG_INPUTS]; static MedianFilter median_filter[NUM_ANALOG_INPUTS];
data = median_filter[active_adc].update((uint16_t)data); data = median_filter[adc_channel].update(data);
#endif #endif
#ifdef ADC_USE_LOWPASS_FILTER #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 #endif
return ((data >> 6) & 0x3ff); // 10bit
return ((data >> 2) & 0x3ff); // return 10bit value as Marlin expects
} }
#define SBIT_CNTEN 0 #define SBIT_CNTEN 0
@ -187,8 +267,8 @@ void HAL_pwm_init(void) {
LPC_PWM1->TCR = _BV(SBIT_CNTEN) | _BV(SBIT_PWMEN); LPC_PWM1->TCR = _BV(SBIT_CNTEN) | _BV(SBIT_PWMEN);
LPC_PWM1->PR = 0x0; // No prescalar LPC_PWM1->PR = 0x0; // No prescalar
LPC_PWM1->MCR = _BV(SBIT_PWMMR0R); // Reset on PWMMR0, reset TC if it matches MR0 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->MR0 = 255; // set PWM cycle(Ton+Toff)=255)
LPC_PWM1->MR5 = 0; /* Set 50% Duty Cycle for the channels */ LPC_PWM1->MR5 = 0; // Set 50% Duty Cycle for the channels
LPC_PWM1->MR6 = 0; LPC_PWM1->MR6 = 0;
// Trigger the latch Enable Bits to load the new Match Values MR0, MR5, MR6 // Trigger the latch Enable Bits to load the new Match Values MR0, MR5, MR6

2
Marlin/src/HAL/HAL_LPC1768/HAL_LCD_pin_routines.c

@ -34,7 +34,7 @@
#include <LPC17xx.h> #include <LPC17xx.h>
#include <lpc17xx_pinsel.h> #include <lpc17xx_pinsel.h>
#include "src/core/macros.h" #include "../../src/core/macros.h"
//#include "pinmapping.h" //#include "pinmapping.h"
#define LPC_PORT_OFFSET (0x0020) #define LPC_PORT_OFFSET (0x0020)

2
Marlin/src/module/temperature.h

@ -90,7 +90,7 @@ enum ADCSensorState {
#if HAS_PID_HEATING #if HAS_PID_HEATING
#define PID_K2 (1.0-PID_K1) #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 // Apply the scale factors to the PID values
#define scalePID_i(i) ( (i) * PID_dT ) #define scalePID_i(i) ( (i) * PID_dT )

Loading…
Cancel
Save