PAT9125 - sampled only Y value, new debug code D9125.

FSensor - synchronous sampling and new algorithm
build number = 103

Firmware/Configuration.h

@@ -9,7 +9,7 @@
 
 // Firmware version
 #define FW_version "3.0.12-RC2"
-#define FW_build   102
+#define FW_build   103
 #define FW_version_build FW_version " b" STR(FW_build)
 
 

Firmware/Configuration_prusa.h

@@ -83,6 +83,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
 //DEBUG
 #define DEBUG_DCODES //D codes
 #if 1
+#define DEBUG_FSENSOR_LOG          //Reports fsensor status to serial
 //#define DEBUG_CRASHDET_COUNTERS  //Display crash-detection counters on LCD
 //#define DEBUG_RESUME_PRINT       //Resume/save print debug enable 
 //#define DEBUG_UVLO_AUTOMATIC_RECOVER // Power panic automatic recovery debug output 

Firmware/Dcodes.cpp

@@ -1,6 +1,7 @@
 #include "Dcodes.h"
 #include "Marlin.h"
 #include "cmdqueue.h"
+#include "pat9125.h"
 
 inline void serial_print_hex_nibble(uint8_t val)
 {
@@ -189,4 +190,57 @@ void dcode_4()
 	}
 }
 
+void dcode_9125()
+{
+	MYSERIAL.println("D9125 - PAT9125");
+	if ((strchr_pointer[1+4] == '?') || (strchr_pointer[1+4] == 0))
+	{
+		MYSERIAL.print("res_x=");
+		MYSERIAL.print(pat9125_xres, DEC);
+		MYSERIAL.print(" res_y=");
+		MYSERIAL.print(pat9125_yres, DEC);
+		MYSERIAL.print(" x=");
+		MYSERIAL.print(pat9125_x, DEC);
+		MYSERIAL.print(" y=");
+		MYSERIAL.print(pat9125_y, DEC);
+		MYSERIAL.print(" b=");
+		MYSERIAL.print(pat9125_b, DEC);
+		MYSERIAL.print(" s=");
+		MYSERIAL.println(pat9125_s, DEC);
+		return;
+	}
+	if (strchr_pointer[1+4] == '!')
+	{
+		pat9125_update();
+		MYSERIAL.print("x=");
+		MYSERIAL.print(pat9125_x, DEC);
+		MYSERIAL.print(" y=");
+		MYSERIAL.print(pat9125_y, DEC);
+		MYSERIAL.print(" b=");
+		MYSERIAL.print(pat9125_b, DEC);
+		MYSERIAL.print(" s=");
+		MYSERIAL.println(pat9125_s, DEC);
+		return;
+	}
+	if (code_seen('R'))
+	{
+		unsigned char res = (int)code_value();
+		MYSERIAL.print("pat9125_init(xres=yres=");
+		MYSERIAL.print(res, DEC);
+		MYSERIAL.print(")=");
+		MYSERIAL.println(pat9125_init(res, res), DEC);
+	}
+	if (code_seen('X'))
+	{
+		pat9125_x = (int)code_value();
+		MYSERIAL.print("pat9125_x=");
+		MYSERIAL.print(pat9125_x, DEC);
+	}
+	if (code_seen('Y'))
+	{
+		pat9125_y = (int)code_value();
+		MYSERIAL.print("pat9125_y=");
+		MYSERIAL.print(pat9125_y, DEC);
+	}
+}
 

Firmware/Dcodes.h

@@ -7,5 +7,7 @@ extern void dcode_2();
 extern void dcode_3();
 extern void dcode_4();
 
+extern void dcode_9125();
+
 
 #endif //DCODES_H

Firmware/Marlin_main.cpp

@@ -988,6 +988,8 @@ void setup()
 
 	check_babystep(); //checking if Z babystep is in allowed range
 	setup_uvlo_interrupt();
+
+	fsensor_setup_interrupt();
 	
 #ifndef DEBUG_DISABLE_STARTMSGS
 
@@ -5713,6 +5715,8 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
 		dcode_3(); break;
 	case 4: // D4 - Read/Write PIN
 		dcode_4(); break;
+	case 9125: // D9125 - PAT9125
+		dcode_9125(); break;
 	case 5:
 		MYSERIAL.println("D5 - Test");
 		if (code_seen('P'))

Firmware/fsensor.cpp

@@ -6,14 +6,20 @@
 #include "pat9125.h"
 #include "planner.h"
 
+//#include "LiquidCrystal.h"
+//extern LiquidCrystal lcd;
+
+
+#define FSENSOR_ERR_MAX      5  //filament sensor max error count
+#define FSENSOR_INT_PIN     63  //filament sensor interrupt pin
+#define FSENSOR_CHUNK_LEN  560  //filament sensor chunk length in steps
+
 extern void stop_and_save_print_to_ram(float z_move, float e_move);
 extern void restore_print_from_ram_and_continue(float e_move);
-extern long st_get_position(uint8_t axis);
+extern int8_t FSensorStateMenu;
-
 
 void fsensor_stop_and_save_print()
 {
-//	stop_and_save_print_to_ram(10, -0.8); //XY - no change, Z 10mm up, E 0.8mm in
 	stop_and_save_print_to_ram(0, 0); //XYZE - no change
 }
 
@@ -22,34 +28,22 @@ void fsensor_restore_print_and_continue()
 	restore_print_from_ram_and_continue(0); //XYZ = orig, E - no change
 }
 
-
+uint8_t fsensor_int_pin = FSENSOR_INT_PIN;
+int16_t fsensor_chunk_len = FSENSOR_CHUNK_LEN;
 bool fsensor_enabled = true;
-bool fsensor_ignore_error = true;
+//bool fsensor_ignore_error = true;
 bool fsensor_M600 = false;
-long fsensor_prev_pos_e = 0;
 uint8_t fsensor_err_cnt = 0;
-
+int16_t fsensor_st_cnt = 0;
-#define FSENS_ESTEPS 280  //extruder resolution [steps/mm]
-//#define FSENS_MINDEL 560  //filament sensor min delta [steps] (3mm)
-#define FSENS_MINDEL 280  //filament sensor min delta [steps] (3mm)
-#define FSENS_MINFAC 3    //filament sensor minimum factor [count/mm]
-//#define FSENS_MAXFAC 50   //filament sensor maximum factor [count/mm]
-#define FSENS_MAXFAC 40   //filament sensor maximum factor [count/mm]
-//#define FSENS_MAXERR 2    //filament sensor max error count
-#define FSENS_MAXERR 5    //filament sensor max error count
-
-extern int8_t FSensorStateMenu;
 
 
 void fsensor_enable()
 {
 	MYSERIAL.println("fsensor_enable");
-	pat9125_y = 0;
-	fsensor_prev_pos_e = st_get_position(E_AXIS);
-	fsensor_err_cnt = 0;
 	fsensor_enabled = true;
-	fsensor_ignore_error = true;
+//	fsensor_ignore_error = true;
 	fsensor_M600 = false;
+	fsensor_err_cnt = 0;
 	eeprom_update_byte((uint8_t*)EEPROM_FSENSOR, 0xFF); 
 	FSensorStateMenu = 1;
 }
@@ -62,18 +56,7 @@ void fsensor_disable()
 	FSensorStateMenu = 0;
 }
 
-#include "LiquidCrystal.h"
+void pciSetup(byte pin)
-extern LiquidCrystal lcd;
-
-//bool pat9125_change = ((old_x != pat9125_x) || (old_y != pat9125_y));
-
-uint8_t fsensor_int_pin = 63;
-int16_t fsensor_steps_e = 0;
-int16_t fsensor_y_old = 0;
-
-
-
-void pciSetup(byte pin) 
 {
 	*digitalPinToPCMSK(pin) |= bit (digitalPinToPCMSKbit(pin)); // enable pin
 	PCIFR |= bit (digitalPinToPCICRbit(pin)); // clear any outstanding interrupt
@@ -90,109 +73,82 @@ void fsensor_setup_interrupt()
 	pciSetup(fsensor_int_pin);
 }
 
-void fsensor_interrupt_raise()
-{
-	digitalWrite(fsensor_int_pin, LOW);
-}
-
 ISR(PCINT2_vect)
 {
+//	return;
+	int st_cnt = fsensor_st_cnt;
+	fsensor_st_cnt = 0;
 	sei();
-	SERIAL_ECHOLNPGM("PCINT2");
 	*digitalPinToPCMSK(fsensor_int_pin) &= ~bit(digitalPinToPCMSKbit(fsensor_int_pin));
 	digitalWrite(fsensor_int_pin, HIGH);
 	*digitalPinToPCMSK(fsensor_int_pin) |= bit(digitalPinToPCMSKbit(fsensor_int_pin));
-//	PCIFR |= bit (digitalPinToPCICRbit(fsensor_int_pin)); // clear any outstanding interrupt
+	pat9125_update_y();
-//	pat9125_update();
+	if (st_cnt != 0)
-	pat9125_update();
+	{
-	MYSERIAL.print("steps_e=");
+#ifdef DEBUG_FSENSOR_LOG
-	MYSERIAL.print(fsensor_steps_e, DEC);
+		MYSERIAL.print("cnt=");
-	MYSERIAL.print(" dy=");
+		MYSERIAL.print(st_cnt, DEC);
-	MYSERIAL.println(pat9125_y - fsensor_y_old, DEC);
+		MYSERIAL.print(" dy=");
-	fsensor_y_old = pat9125_y;
+		MYSERIAL.print(pat9125_y, DEC);
+#endif //DEBUG_FSENSOR_LOG
+		if (st_cnt != 0)
+		{
+			if( (pat9125_y == 0) || ((pat9125_y > 0) && (st_cnt < 0)) || ((pat9125_y < 0) && (st_cnt > 0)))
+			{ //invalid movement
+				fsensor_err_cnt++;
+#ifdef DEBUG_FSENSOR_LOG
+				MYSERIAL.print("\tNG ! err=");
+				MYSERIAL.println(fsensor_err_cnt, DEC);
+#endif //DEBUG_FSENSOR_LOG
+			}
+			else
+			{ //propper movement
+				if (fsensor_err_cnt > 0)
+					fsensor_err_cnt--;
+#ifdef DEBUG_FSENSOR_LOG
+				MYSERIAL.print("\tOK    err=");
+				MYSERIAL.println(fsensor_err_cnt, DEC);
+#endif //DEBUG_FSENSOR_LOG
+			}
+		}
+		else
+		{ //no movement
+#ifdef DEBUG_FSENSOR_LOG
+			MYSERIAL.println("\tOK 0");
+#endif //DEBUG_FSENSOR_LOG
+		}
+	}
+	pat9125_y = 0;
+	return;
 }
 
-void fsensor_st_end_block(block_t* bl)
+void fsensor_st_block_begin(block_t* bl)
 {
-//	return;
+	if ((fsensor_st_cnt > 0) && (bl->direction_bits & 0x8))
-	fsensor_steps_e = bl->steps_e;
+		digitalWrite(fsensor_int_pin, LOW);
-	digitalWrite(fsensor_int_pin, LOW);
+	if ((fsensor_st_cnt < 0) && !(bl->direction_bits & 0x8))
+		digitalWrite(fsensor_int_pin, LOW);
 }
 
-void fsensor_st_new_block(block_t* bl)
+void fsensor_st_block_chunk(block_t* bl, int cnt)
 {
-//	return;
+	fsensor_st_cnt += (bl->direction_bits & 0x8)?-cnt:cnt;
-//	fsensor_steps_e = bl->steps_e;
+	if ((fsensor_st_cnt >= fsensor_chunk_len) || (fsensor_st_cnt <= -fsensor_chunk_len))
-//	digitalWrite(fsensor_int_pin, LOW);
+		digitalWrite(fsensor_int_pin, LOW);
 }
 
 void fsensor_update()
 {
-//	return;
 	if (!fsensor_enabled) return;
-	long pos_e = st_get_position(E_AXIS); //current position
+	if (fsensor_err_cnt > FSENSOR_ERR_MAX)
-	int old_x = pat9125_x;
-	int old_y = pat9125_y;
-	pat9125_update();
-/*	bool pat9125_change = ((old_x != pat9125_x) || (old_y != pat9125_y));
-	static uint32_t checktime = 0;
-	pat9125_change |= (millis() - checktime > 250);
-#ifdef DEBUG_PAT9125_COUNTERS
-	if (pat9125_change)
 	{
-		lcd.setCursor(2, 3);
+		MYSERIAL.println("fsensor_update (fsensor_err_cnt > FSENSOR_ERR_MAX)");
-		lcd.print(pat9125_x, DEC);
+/*		if (fsensor_ignore_error)
-		lcd.print(' ');
-		lcd.print(pat9125_y, DEC);
-		lcd.print(' ');
-		lcd.print(pos_e, DEC);
-		lcd.print(' ');
-		checktime = millis();
-	}
-#endif DEBUG_PAT9125_COUNTERS
-	return;*/
-
-	long del_e = pos_e - fsensor_prev_pos_e; //delta
-	if (abs(del_e) < FSENS_MINDEL) return;
-	float de = ((float)del_e / FSENS_ESTEPS);
-	int cmin = de * FSENS_MINFAC;
-	int cmax = de * FSENS_MAXFAC;
-	int cnt = -pat9125_y;
-	fsensor_prev_pos_e = pos_e;
-	pat9125_y = 0;
-	bool err = false;
-	if ((del_e > 0) && ((cnt < cmin) || (cnt > cmax))) err = true;
-	if ((del_e < 0) && ((cnt > cmin) || (cnt < cmax))) err = true;
-	if (err)
-		fsensor_err_cnt++;
-	else
-		fsensor_err_cnt = 0;
-
-/**/
-	MYSERIAL.print("pos_e=");
-	MYSERIAL.print(pos_e);
-	MYSERIAL.print(" de=");
-	MYSERIAL.print(de);
-	MYSERIAL.print(" cmin=");
-	MYSERIAL.print((int)cmin);
-	MYSERIAL.print(" cmax=");
-	MYSERIAL.print((int)cmax);
-	MYSERIAL.print(" cnt=");
-	MYSERIAL.print((int)cnt);
-	MYSERIAL.print(" err=");
-	MYSERIAL.println((int)fsensor_err_cnt);/**/
-
-//	return;
-
-	if (fsensor_err_cnt > FSENS_MAXERR)
-	{
-		MYSERIAL.println("fsensor_update (fsensor_err_cnt > FSENS_MAXERR)");
-		if (fsensor_ignore_error)
 		{
 			MYSERIAL.println("fsensor_update - error ignored)");
 			fsensor_ignore_error = false;
 		}
-		else
+		else*/
 		{
 			MYSERIAL.println("fsensor_update - ERROR!!!");
 			fsensor_stop_and_save_print();

Firmware/fsensor.h

@@ -1,19 +1,30 @@
 #ifndef FSENSOR_H
 #define FSENSOR_H
 
+#include "planner.h"
 
+//save restore printing
 extern void fsensor_stop_and_save_print();
 extern void fsensor_restore_print_and_continue();
+
+//enable/disable
 extern void fsensor_enable();
 extern void fsensor_disable();
-extern void fsensor_setup_interrupt();
+
+//update (perform M600 on filament runout)
 extern void fsensor_update();
 
+//setup pin-change interrupt
+extern void fsensor_setup_interrupt();
 
+//callbacks from stepper
+extern void fsensor_st_block_begin(block_t* bl);
+extern void fsensor_st_block_chunk(block_t* bl, int cnt);
+
+//minimum meassured chunk length in steps
+extern int16_t fsensor_chunk_len;
+//M600 in progress
 extern bool fsensor_M600;
-extern int16_t fsensor_steps_e;
-extern int16_t fsensor_y_old;
-
 
 
 #endif //FSENSOR_H

Firmware/pat9125.cpp

@@ -17,9 +17,12 @@
 
 unsigned char pat9125_PID1 = 0;
 unsigned char pat9125_PID2 = 0;
+unsigned char pat9125_xres = 0;
+unsigned char pat9125_yres = 0;
 int pat9125_x = 0;
 int pat9125_y = 0;
-int pat9125_b = 0;
+unsigned char pat9125_b = 0;
+unsigned char pat9125_s = 0;
 
 int pat9125_init(unsigned char xres, unsigned char yres)
 {
@@ -32,14 +35,17 @@ int pat9125_init(unsigned char xres, unsigned char yres)
 #ifdef PAT9125_HWI2C
 	Wire.begin();
 #endif //PAT9125_HWI2C
+	pat9125_xres = xres;
+	pat9125_yres = yres;
 	pat9125_PID1 = pat9125_rd_reg(PAT9125_PID1);
 	pat9125_PID2 = pat9125_rd_reg(PAT9125_PID2);
 	if ((pat9125_PID1 != 0x31) || (pat9125_PID2 != 0x91))
 	{
 		return 0;
 	}
-    pat9125_wr_reg(PAT9125_RES_X, xres);
+    pat9125_wr_reg(PAT9125_RES_X, pat9125_xres);
-    pat9125_wr_reg(PAT9125_RES_Y, yres);
+    pat9125_wr_reg(PAT9125_RES_Y, pat9125_yres);
+//	pat9125_wr_reg(PAT9125_ORIENTATION, 0x04 | (xinv?0x08:0) | (yinv?0x10:0)); //!? direction switching does not work
 	return 1;
 }
 
@@ -49,6 +55,7 @@ int pat9125_update()
 	{
 		unsigned char ucMotion = pat9125_rd_reg(PAT9125_MOTION);
 		pat9125_b = pat9125_rd_reg(PAT9125_FRAME);
+		pat9125_s = pat9125_rd_reg(PAT9125_SHUTTER);
 		if (ucMotion & 0x80)
 		{
 			unsigned char ucXL = pat9125_rd_reg(PAT9125_DELTA_XL);
@@ -59,7 +66,25 @@ int pat9125_update()
 			if (iDX & 0x800) iDX -= 4096;
 			if (iDY & 0x800) iDY -= 4096;
 			pat9125_x += iDX;
-			pat9125_y += iDY;
+			pat9125_y -= iDY; //negative number, because direction switching does not work
+			return 1;
+		}
+	}
+	return 0;
+}
+
+int pat9125_update_y()
+{
+	if ((pat9125_PID1 == 0x31) && (pat9125_PID2 == 0x91))
+	{
+		unsigned char ucMotion = pat9125_rd_reg(PAT9125_MOTION);
+		if (ucMotion & 0x80)
+		{
+			unsigned char ucYL = pat9125_rd_reg(PAT9125_DELTA_YL);
+			unsigned char ucXYH = pat9125_rd_reg(PAT9125_DELTA_XYH);
+			int iDY = ucYL | ((ucXYH << 8) & 0xf00);
+			if (iDY & 0x800) iDY -= 4096;
+			pat9125_y -= iDY; //negative number, because direction switching does not work
 			return 1;
 		}
 	}

Firmware/pat9125.h

@@ -27,12 +27,17 @@
 extern unsigned char pat9125_PID1;
 extern unsigned char pat9125_PID2;
 
+extern unsigned char pat9125_xres;
+extern unsigned char pat9125_yres;
+
 extern int pat9125_x;
 extern int pat9125_y;
-extern int pat9125_b;
+extern unsigned char pat9125_b;
+extern unsigned char pat9125_s;
 
 extern int pat9125_init(unsigned char xres, unsigned char yres);
 extern int pat9125_update();
+extern int pat9125_update_y();
 
 extern unsigned char pat9125_rd_reg(unsigned char addr);
 extern void pat9125_wr_reg(unsigned char addr, unsigned char data);

Firmware/stepper.cpp

@@ -37,7 +37,8 @@
 #endif //TMC2130
 
 #ifdef PAT9125
-extern uint8_t fsensor_err_cnt;
+#include "fsensor.h"
+int fsensor_counter = 0; //counter for e-steps
 #endif //PAT9125
 
 //===========================================================================
@@ -370,7 +371,11 @@ void isr() {
     // Anything in the buffer?
     current_block = plan_get_current_block();
     if (current_block != NULL) {
-      // The busy flag is set by the plan_get_current_block() call.
+#ifdef PAT9125
+	  fsensor_counter = 0;
+      fsensor_st_block_begin(current_block);
+#endif //PAT9125
+	  // The busy flag is set by the plan_get_current_block() call.
       // current_block->busy = true;
       trapezoid_generator_reset();
       counter_x = -(current_block->step_event_count >> 1);
@@ -716,6 +721,9 @@ void isr() {
           counter_e -= current_block->step_event_count;
           count_position[E_AXIS]+=count_direction[E_AXIS];
           WRITE_E_STEP(INVERT_E_STEP_PIN);
+#ifdef PAT9125
+          fsensor_counter++;
+#endif //PAT9125
         }
 #endif
         
@@ -799,13 +807,20 @@ void isr() {
     if (step_events_completed >= current_block->step_event_count) {
 
 #ifdef PAT9125
-		if (current_block->steps_e < 0) //black magic - decrement filament sensor errors for negative extruder move
+      fsensor_st_block_chunk(current_block, fsensor_counter);
-			if (fsensor_err_cnt) fsensor_err_cnt--;
+	  fsensor_counter = 0;
 #endif //PAT9125
 
       current_block = NULL;
       plan_discard_current_block();
     }
+#ifdef PAT9125
+	else if (fsensor_counter >= fsensor_chunk_len)
+	{
+      fsensor_st_block_chunk(current_block, fsensor_counter);
+	  fsensor_counter = 0;
+	}
+#endif //PAT9125
   }
 #ifdef TMC2130
 	tmc2130_st_isr(LastStepMask);
@@ -834,6 +849,10 @@ void advance_isr() {
           WRITE(E0_STEP_PIN, !INVERT_E_STEP_PIN);
           e_steps < 0 ? ++e_steps : --e_steps;
           WRITE(E0_STEP_PIN, INVERT_E_STEP_PIN);
+#ifdef PAT9125
+		  fsensor_counter++;
+#endif //PAT9125
+
       }
   }
 }