From e84f82a6757ec3bfe3a9473b71f145775f631bae Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Wed, 5 Feb 2020 15:46:45 +0100 Subject: [PATCH] Rewrite filament sensor PAT9125 error handling Rewrite the logic behind the "chunking"/error count behind the PAT9125. Basic idea: check the _direction_ of movement returned by the optical sensor and compare it to the direction of the stepper. To avoid doing this continuosly (and because the optical sensor doesn't necessarily have the accuracy to track small distances), do so in chunks. Each time a chunk doesn't match the expected direction, increase the error count. Several improvements were done to the previous code: - Increase the chunk window: this ensures that a filament with poor response returns an usable direction, while also moving the average return values from the sensor in the middle of the 12 bits available for maximum effectiveness. - Since the returned values are more reliable, reduce the error count (1.25mm*4 = ~5mm before runout detection) - Track _both_ positive and negative movement, although only trigger errors during extrusion (necessary due to several assumptions made in the mmu/unloading code) - Do not reset the counters for each block: accumulate distances correctly, allowing detection of any block lenght. --- Firmware/config.h | 3 +- Firmware/fsensor.cpp | 104 +++++++++++++++++++++---------------------- Firmware/stepper.cpp | 1 - 3 files changed, 53 insertions(+), 55 deletions(-) diff --git a/Firmware/config.h b/Firmware/config.h index 241a2f20b..5c8f7cd98 100644 --- a/Firmware/config.h +++ b/Firmware/config.h @@ -34,7 +34,8 @@ //#define PAT9125_I2C_ADDR 0x79 //ID=HI //#define PAT9125_I2C_ADDR 0x73 //ID=NC #define PAT9125_XRES 0 -#define PAT9125_YRES 240 +#define PAT9125_YRES 240 // maximum resolution (5*X cpi) +#define PAT9124_YRES_MM (5*PAT9125_YRES/25.4) // counts per mm //SM4 configuration #define SM4_DEFDELAY 500 //default step delay [us] diff --git a/Firmware/fsensor.cpp b/Firmware/fsensor.cpp index 3c42a3424..ea171906f 100755 --- a/Firmware/fsensor.cpp +++ b/Firmware/fsensor.cpp @@ -18,8 +18,8 @@ //! @name Basic parameters //! @{ -#define FSENSOR_CHUNK_LEN 0.64F //!< filament sensor chunk length 0.64mm -#define FSENSOR_ERR_MAX 9 //!< filament sensor maximum error count for runout detection +#define FSENSOR_CHUNK_LEN 1.25 //!< filament sensor chunk length (mm) +#define FSENSOR_ERR_MAX 4 //!< filament sensor maximum error/chunk count for runout detection #define FSENSOR_SOFTERR_CMAX 3 //!< number of contiguous soft failures before a triggering a runout #define FSENSOR_SOFTERR_DELTA 30000 //!< maximum interval (ms) to consider soft failures contiguous @@ -64,8 +64,6 @@ bool fsensor_oq_meassure_enabled = false; uint8_t fsensor_err_cnt = 0; //! variable for accumulating step count (updated callbacks from stepper and ISR) int16_t fsensor_st_cnt = 0; -//! last dy value from pat9125 sensor (used in ISR) -int16_t fsensor_dy_old = 0; //! count of total sensor "soft" failures (filament status checks) uint8_t fsensor_softfail = 0; //! timestamp of last soft failure @@ -226,7 +224,6 @@ bool fsensor_enable(bool bUpdateEEPROM) fsensor_watch_runout = true; fsensor_oq_meassure = false; fsensor_reset_err_cnt(); - fsensor_dy_old = 0; eeprom_update_byte((uint8_t*)EEPROM_FSENSOR, fsensor_enabled ? 0x01 : 0x00); FSensorStateMenu = fsensor_enabled ? 1 : 0; } @@ -477,53 +474,56 @@ ISR(FSENSOR_INT_PIN_VECT) fsensor_not_responding = true; printf_P(ERRMSG_PAT9125_NOT_RESP, 1); } + if (st_cnt != 0) - { //movement - if (st_cnt > 0) //positive movement - { - if (pat9125_y < 0) - { - fsensor_err_cnt++; - } - else if (pat9125_y > 0) - { - if (fsensor_err_cnt) - fsensor_err_cnt--; - } - else //(pat9125_y == 0) - if (((fsensor_dy_old <= 0) || (fsensor_err_cnt)) && (st_cnt > (fsensor_chunk_len >> 1))) - fsensor_err_cnt++; - if (fsensor_oq_meassure) - { - if (fsensor_oq_skipchunk) - { - fsensor_oq_skipchunk--; - fsensor_err_cnt = 0; - } - else - { - if (st_cnt == fsensor_chunk_len) - { - if (pat9125_y > 0) if (fsensor_oq_yd_min > pat9125_y) fsensor_oq_yd_min = (fsensor_oq_yd_min + pat9125_y) / 2; - if (pat9125_y >= 0) if (fsensor_oq_yd_max < pat9125_y) fsensor_oq_yd_max = (fsensor_oq_yd_max + pat9125_y) / 2; - } - fsensor_oq_samples++; - fsensor_oq_st_sum += st_cnt; - if (pat9125_y > 0) fsensor_oq_yd_sum += pat9125_y; - if (fsensor_err_cnt > old_err_cnt) - fsensor_oq_er_sum += (fsensor_err_cnt - old_err_cnt); - if (fsensor_oq_er_max < fsensor_err_cnt) - fsensor_oq_er_max = fsensor_err_cnt; - fsensor_oq_sh_sum += pat9125_s; - } - } - } - else //negative movement - { - } - } - else - { //no movement + { + // movement was planned, check for sensor movement + int8_t st_dir = st_cnt >= 0; + int8_t pat9125_dir = pat9125_y >= 0; + + if (pat9125_y == 0) + { + // no movement detected: increase error count only when extruding, since fast retracts + // cannot always be seen. We also need to ensure that no runout is generated while + // retracting as it's not currently handled everywhere + if (st_dir) ++fsensor_err_cnt; + } + else if (pat9125_dir != st_dir) + { + // detected direction opposite of motor movement + if (st_dir) ++fsensor_err_cnt; + } + else if (pat9125_dir == st_dir) + { + // direction agreeing with planned movement + if (fsensor_err_cnt) --fsensor_err_cnt; + } + + if (st_dir && fsensor_oq_meassure) + { + // extruding with quality assessment + if (fsensor_oq_skipchunk) + { + fsensor_oq_skipchunk--; + fsensor_err_cnt = 0; + } + else + { + if (st_cnt == fsensor_chunk_len) + { + if (pat9125_y > 0) if (fsensor_oq_yd_min > pat9125_y) fsensor_oq_yd_min = (fsensor_oq_yd_min + pat9125_y) / 2; + if (pat9125_y >= 0) if (fsensor_oq_yd_max < pat9125_y) fsensor_oq_yd_max = (fsensor_oq_yd_max + pat9125_y) / 2; + } + fsensor_oq_samples++; + fsensor_oq_st_sum += st_cnt; + if (pat9125_y > 0) fsensor_oq_yd_sum += pat9125_y; + if (fsensor_err_cnt > old_err_cnt) + fsensor_oq_er_sum += (fsensor_err_cnt - old_err_cnt); + if (fsensor_oq_er_max < fsensor_err_cnt) + fsensor_oq_er_max = fsensor_err_cnt; + fsensor_oq_sh_sum += pat9125_s; + } + } } #ifdef DEBUG_FSENSOR_LOG @@ -534,9 +534,7 @@ ISR(FSENSOR_INT_PIN_VECT) } #endif //DEBUG_FSENSOR_LOG - fsensor_dy_old = pat9125_y; pat9125_y = 0; - _lock = false; return; } diff --git a/Firmware/stepper.cpp b/Firmware/stepper.cpp index 93e4ad8fa..6e0937cfd 100644 --- a/Firmware/stepper.cpp +++ b/Firmware/stepper.cpp @@ -422,7 +422,6 @@ FORCE_INLINE void stepper_next_block() count_direction[E_AXIS] = 1; } #if defined(FILAMENT_SENSOR) && defined(PAT9125) - fsensor_counter = 0; fsensor_st_block_begin(count_direction[E_AXIS] < 0); #endif //FILAMENT_SENSOR }