Improvements and bug fixes in sensor delay buffer for filament sensor code

Improvement to avoid reinitializing delay buffer with every print. Fixed
issues in buffer indexing and memory out of bounds due to floating point
imprecision.  Simplified the code by avoiding conversion to standard
diameter and 1cu mm extrusion, which caused complications in determining
mm extruded.

Marlin/Marlin.h

@@ -230,8 +230,8 @@ extern unsigned char fanSpeedSoftPwm;
 #endif
 
 #ifdef FILAMENT_SENSOR
-  extern volatile float filament_width_nominal;  //holds the theoretical filament diameter ie., 3.00 or 1.75 
+  extern float filament_width_nominal;  //holds the theoretical filament diameter ie., 3.00 or 1.75 
-  extern volatile bool filament_sensor;  //indicates that filament sensor readings should control extrusion  
+  extern bool filament_sensor;  //indicates that filament sensor readings should control extrusion  
   extern float filament_width_meas; //holds the filament diameter as accurately measured 
   extern signed char measurement_delay[];  //ring buffer to delay measurement
   extern int delay_index1, delay_index2;  //index into ring buffer

Marlin/Marlin_main.cpp

@@ -302,12 +302,12 @@ bool cancel_heatup = false ;
 
 #ifdef FILAMENT_SENSOR
   //Variables for Filament Sensor input 
-  volatile float filament_width_nominal=DEFAULT_NOMINAL_FILAMENT_DIA;  //Set nominal filament width, can be changed with M404 
+  float filament_width_nominal=DEFAULT_NOMINAL_FILAMENT_DIA;  //Set nominal filament width, can be changed with M404 
-  volatile bool filament_sensor=false;  //M405 turns on filament_sensor control, M406 turns it off 
+  bool filament_sensor=false;  //M405 turns on filament_sensor control, M406 turns it off 
   float filament_width_meas=DEFAULT_MEASURED_FILAMENT_DIA; //Stores the measured filament diameter 
   signed char measurement_delay[MAX_MEASUREMENT_DELAY+1];  //ring buffer to delay measurement  store extruder factor after subtracting 100 
-  int delay_index1=0;
+  int delay_index1=0;  //index into ring buffer
-  int delay_index2=0;  //index into ring buffer
+  int delay_index2=-1;  //index into ring buffer - set to -1 on startup to indicate ring buffer needs to be initialized
   float delay_dist=0; //delay distance counter  
   int meas_delay_cm = MEASUREMENT_DELAY_CM;  //distance delay setting
 #endif
@@ -504,6 +504,7 @@ void servo_init()
   #endif
 }
 
+
 void setup()
 {
   setup_killpin();
@@ -554,6 +555,7 @@ void setup()
   setup_photpin();
   servo_init();
   
+
   lcd_init();
   _delay_ms(1000);	// wait 1sec to display the splash screen
 
@@ -2333,12 +2335,8 @@ void process_commands()
           }
         } else {
           //reserved for setting filament diameter via UFID or filament measuring device
-        	if(active_extruder == FILAMENT_SENSOR_EXTRUDER_NUM){
+          break;
-        		radius = analog2widthFil() * 0.5;
+        
-        		area = M_PI * pow(radius, 2);
-        	}else{
-        		area = 1.0;
-        	}
           
         }
         tmp_extruder = active_extruder;
@@ -2816,14 +2814,18 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
        if(meas_delay_cm> MAX_MEASUREMENT_DELAY)
        	meas_delay_cm = MAX_MEASUREMENT_DELAY;
     
-    filament_sensor = true ; 
+       if(delay_index2 == -1)  //initialize the ring buffer if it has not been done since startup
-    int temp_ratio = widthFil_to_size_ratio(); 
+    	   {
+    	   int temp_ratio = widthFil_to_size_ratio(); 
        	    
-    for (delay_index1=0; delay_index1<(MAX_MEASUREMENT_DELAY+1); ++delay_index1 ){
+       	    for (delay_index1=0; delay_index1<(MAX_MEASUREMENT_DELAY+1); ++delay_index1 ){
-              measurement_delay[delay_index1]=temp_ratio-100;  //subtract 100 to scale within a signed byte
+       	              measurement_delay[delay_index1]=temp_ratio-100;  //subtract 100 to scale within a signed byte
-        }
+       	        }
-    delay_index1=0;
+       	    delay_index1=0;
-    delay_index2=0;
+       	    delay_index2=0;	
+    	   }
+    
+    filament_sensor = true ; 
     
     //SERIAL_PROTOCOLPGM("Filament dia (measured mm):"); 
     //SERIAL_PROTOCOL(filament_width_meas); 
@@ -2841,7 +2843,7 @@ case 404:  //M404 Enter the nominal filament width (3mm, 1.75mm ) N<3.0> or disp
     case 407:   //M407 Display measured filament diameter 
     { 
      
-    filament_width_meas = code_value(); 
+    
     
     SERIAL_PROTOCOLPGM("Filament dia (measured mm):"); 
     SERIAL_PROTOCOLLN(filament_width_meas);   

Marlin/planner.cpp

@@ -744,14 +744,24 @@ block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-positi
 #ifdef FILAMENT_SENSOR
   //FMM update ring buffer used for delay with filament measurements
   
-    if(filament_sensor && (extruder==0))  //only for extruder 0
+  
+    if((extruder==FILAMENT_SENSOR_EXTRUDER_NUM) && (delay_index2 > -1))  //only for extruder with filament sensor and if ring buffer is initialized
   	  {
     delay_dist = delay_dist + delta_mm[E_AXIS];  //increment counter with next move in e axis
-    if (delay_dist> (10*(MAX_MEASUREMENT_DELAY+1)))  //check if counter is over max buffer size in mm
+  
+    while (delay_dist >= (10*(MAX_MEASUREMENT_DELAY+1)))  //check if counter is over max buffer size in mm
       	  delay_dist = delay_dist - 10*(MAX_MEASUREMENT_DELAY+1);  //loop around the buffer
-      if(delay_dist<0)
+    while (delay_dist<0)
     	  delay_dist = delay_dist + 10*(MAX_MEASUREMENT_DELAY+1); //loop around the buffer
-    delay_index1=delay_dist/10;  //calculate index
+      
+    delay_index1=delay_dist/10.0;  //calculate index
+    
+    //ensure the number is within range of the array after converting from floating point
+    if(delay_index1<0)
+    	delay_index1=0;
+    else if (delay_index1>MAX_MEASUREMENT_DELAY)
+    	delay_index1=MAX_MEASUREMENT_DELAY;
+    	
     if(delay_index1 != delay_index2)  //moved index
   	  {
     	meas_sample=widthFil_to_size_ratio()-100;  //subtract off 100 to reduce magnitude - to store in a signed char
@@ -761,9 +771,15 @@ block->steps_y = labs((target[X_AXIS]-position[X_AXIS]) - (target[Y_AXIS]-positi
   	  delay_index2 = delay_index2 + 1;
   	if(delay_index2>MAX_MEASUREMENT_DELAY)
   			  delay_index2=delay_index2-(MAX_MEASUREMENT_DELAY+1);  //loop around buffer when incrementing
+  	  if(delay_index2<0)
+  		delay_index2=0;
+  	  else if (delay_index2>MAX_MEASUREMENT_DELAY)
+  		delay_index2=MAX_MEASUREMENT_DELAY;  
+  	  
   	  measurement_delay[delay_index2]=meas_sample;
   	  }
     	
+    
   	  }
 #endif
 

Marlin/temperature.cpp

@@ -617,9 +617,16 @@ void manage_heater()
 			  meas_shift_index = meas_shift_index + (MAX_MEASUREMENT_DELAY+1);  //loop around buffer if needed
 		  
 		  //get the delayed info and add 100 to reconstitute to a percent of the nominal filament diameter
-		  //then adjust as a factor to the Standard Diameter (has an area of 1mm squared)
 		  //then square it to get an area
-		  volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM] = pow((float)(100+measurement_delay[meas_shift_index])/filament_width_nominal*STANDARD_DIA/100.0,2);
+		  
+		  if(meas_shift_index<0)
+			  meas_shift_index=0;
+		  else if (meas_shift_index>MAX_MEASUREMENT_DELAY)
+			  meas_shift_index=MAX_MEASUREMENT_DELAY;
+		  
+		     volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM] = pow((float)(100+measurement_delay[meas_shift_index])/100.0,2);
+		     if (volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM] <0.01)
+		    	 volumetric_multiplier[FILAMENT_SENSOR_EXTRUDER_NUM]=0.01;
 	}
 #endif
 }
@@ -715,6 +722,9 @@ static void updateTemperaturesFromRawValues()
     #ifdef TEMP_SENSOR_1_AS_REDUNDANT
       redundant_temperature = analog2temp(redundant_temperature_raw, 1);
     #endif
+    #ifdef FILAMENT_SENSOR  && (FILWIDTH_PIN > -1)    //check if a sensor is supported 
+      filament_width_meas = analog2widthFil();
+    #endif  
     //Reset the watchdog after we know we have a temperature measurement.
     watchdog_reset();
 
@@ -731,15 +741,11 @@ return current_raw_filwidth/16383.0*5.0;
 //return current_raw_filwidth; 
 } 
  
-// For converting raw Filament Width to an volumetric ratio 
+// For converting raw Filament Width to a ratio 
 int widthFil_to_size_ratio() { 
  
 float temp; 
       
-#if (FILWIDTH_PIN > -1)    //check if a sensor is supported 
-filament_width_meas=current_raw_filwidth/16383.0*5.0; 
-#endif   
- 
 temp=filament_width_meas;
 if(filament_width_meas<MEASURED_LOWER_LIMIT)
 	temp=filament_width_nominal;  //assume sensor cut out