Further fixes of the power panic and g-code & planner queues.

Firmware/Marlin_main.cpp

@@ -1151,6 +1151,7 @@ void loop()
   #endif
   if(buflen)
   {
+    cmdbuffer_front_already_processed = false;
     #ifdef SDSUPPORT
       if(card.saving)
       {
@@ -1173,18 +1174,25 @@ void loop()
       process_commands();
     #endif //SDSUPPORT
 
-      if (! cmdbuffer_front_already_processed)
+    if (! cmdbuffer_front_already_processed && buflen)
 	  {
-		cli();
-    	uint8_t sdlen = 0;
-		if (CMDBUFFER_CURRENT_TYPE == CMDBUFFER_CURRENT_TYPE_SDCARD)
-			sdlen = cmdbuffer[bufindr + 1];
-	    cmdqueue_pop_front();
-		if (sdlen)
-			planner_add_sd_length(sdlen);
-		sei();
+		    cli();
+        union {
+          struct {
+              char lo;
+              char hi;
+          } lohi;
+          uint16_t value;
+        } sdlen;
+        sdlen.value = 0;
+		    if (CMDBUFFER_CURRENT_TYPE == CMDBUFFER_CURRENT_TYPE_SDCARD) {
+			      sdlen.lohi.lo = cmdbuffer[bufindr + 1];
+            sdlen.lohi.hi = cmdbuffer[bufindr + 2];
+        }
+	      cmdqueue_pop_front();
+		    planner_add_sd_length(sdlen.value);
+		    sei();
 	  }
-      cmdbuffer_front_already_processed = false;
   }
 }
   //check heater every n milliseconds
@@ -1498,8 +1506,8 @@ void homeaxis(int axis)
         plan_buffer_line(destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS], feedrate/60, active_extruder);
         st_synchronize();
 
-		axis_is_at_home(axis);
-		destination[axis] = current_position[axis];
+		axis_is_at_home(axis);
+		destination[axis] = current_position[axis];
 		feedrate = 0.0;
 
 		endstops_hit_on_purpose();
@@ -6941,7 +6949,8 @@ void restore_print_from_eeprom() {
 
 bool saved_printing = false;
 uint32_t saved_sdpos = 0;
-uint32_t saved_pos[4] = {0, 0, 0, 0};
+float saved_pos[4] = {0, 0, 0, 0};
+// Feedrate hopefully derived from an active block of the planner at the time the print has been canceled, in mm/min.
 float saved_feedrate2 = 0;
 uint8_t saved_active_extruder = 0;
 bool saved_extruder_under_pressure = false;
@@ -6950,16 +6959,112 @@ void stop_and_save_print_to_ram(float z_move, float e_move)
 {
 	if (saved_printing) return;
 	cli();
+  unsigned char nplanner_blocks = number_of_blocks();
 	saved_sdpos = sdpos_atomic; //atomic sd position of last command added in queue
 	uint16_t sdlen_planner = planner_calc_sd_length(); //length of sd commands in planner
 	saved_sdpos -= sdlen_planner;
 	uint16_t sdlen_cmdqueue = cmdqueue_calc_sd_length(); //length of sd commands in cmdqueue
 	saved_sdpos -= sdlen_cmdqueue;
+
+#if 0
+  SERIAL_ECHOPGM("SDPOS_ATOMIC="); MYSERIAL.println(sdpos_atomic, DEC);
+  SERIAL_ECHOPGM("SDPOS="); MYSERIAL.println(card.get_sdpos(), DEC);
+  SERIAL_ECHOPGM("SDLEN_PLAN="); MYSERIAL.println(sdlen_planner, DEC);
+  SERIAL_ECHOPGM("SDLEN_CMDQ="); MYSERIAL.println(sdlen_cmdqueue, DEC);
+  SERIAL_ECHOPGM("PLANNERBLOCKS="); MYSERIAL.println(int(nplanner_blocks), DEC);
+  SERIAL_ECHOPGM("SDSAVED="); MYSERIAL.println(saved_sdpos, DEC);
+  SERIAL_ECHOPGM("SDFILELEN="); MYSERIAL.println(card.fileSize(), DEC);
+
+  {
+    card.setIndex(saved_sdpos);
+    SERIAL_ECHOLNPGM("Content of planner buffer: ");
+    for (unsigned int idx = 0; idx < sdlen_planner; ++ idx)
+      MYSERIAL.print(char(card.get()));
+    SERIAL_ECHOLNPGM("Content of command buffer: ");
+    for (unsigned int idx = 0; idx < sdlen_cmdqueue; ++ idx)
+      MYSERIAL.print(char(card.get()));
+    SERIAL_ECHOLNPGM("End of command buffer");
+  }
+
+  {
+    // Print the content of the planner buffer, line by line:
+    card.setIndex(saved_sdpos);
+    int8_t iline = 0;
+    for (unsigned char idx = block_buffer_tail; idx != block_buffer_head; idx = (idx + 1) & (BLOCK_BUFFER_SIZE - 1), ++ iline) {
+      SERIAL_ECHOPGM("Planner line (from file): ");
+      MYSERIAL.print(int(iline), DEC);
+      SERIAL_ECHOPGM(", length: ");
+      MYSERIAL.print(block_buffer[idx].sdlen, DEC);
+      SERIAL_ECHOPGM(", steps: (");
+      MYSERIAL.print(block_buffer[idx].steps_x, DEC);
+      SERIAL_ECHOPGM(",");
+      MYSERIAL.print(block_buffer[idx].steps_y, DEC);
+      SERIAL_ECHOPGM(",");
+      MYSERIAL.print(block_buffer[idx].steps_z, DEC);
+      SERIAL_ECHOPGM(",");
+      MYSERIAL.print(block_buffer[idx].steps_e, DEC);
+      SERIAL_ECHOPGM("), events: ");
+      MYSERIAL.println(block_buffer[idx].step_event_count, DEC);
+      for (int len = block_buffer[idx].sdlen; len > 0; -- len)
+        MYSERIAL.print(char(card.get()));
+    }
+  }
+  {
+    // Print the content of the command buffer, line by line:
+    int8_t iline = 0;
+    union {
+        struct {
+            char lo;
+            char hi;
+        } lohi;
+        uint16_t value;
+    } sdlen_single;
+    int _bufindr = bufindr;
+    for (int _buflen  = buflen; _buflen > 0; ++ iline) {
+        if (cmdbuffer[_bufindr] == CMDBUFFER_CURRENT_TYPE_SDCARD) {
+            sdlen_single.lohi.lo = cmdbuffer[_bufindr + 1];
+            sdlen_single.lohi.hi = cmdbuffer[_bufindr + 2];
+        }
+        SERIAL_ECHOPGM("Buffer line (from buffer): ");
+        MYSERIAL.print(int(iline), DEC);
+        SERIAL_ECHOPGM(", type: ");
+        MYSERIAL.print(int(cmdbuffer[_bufindr]), DEC);
+        SERIAL_ECHOPGM(", len: ");
+        MYSERIAL.println(sdlen_single.value, DEC);
+        // Print the content of the buffer line.
+        MYSERIAL.println(cmdbuffer + _bufindr + CMDHDRSIZE);
+
+        SERIAL_ECHOPGM("Buffer line (from file): ");
+        MYSERIAL.print(int(iline), DEC);
+        MYSERIAL.println(int(iline), DEC);
+        for (; sdlen_single.value > 0; -- sdlen_single.value)
+          MYSERIAL.print(char(card.get()));
+
+        if (-- _buflen == 0)
+          break;
+        // First skip the current command ID and iterate up to the end of the string.
+        for (_bufindr += CMDHDRSIZE; cmdbuffer[_bufindr] != 0; ++ _bufindr) ;
+        // Second, skip the end of string null character and iterate until a nonzero command ID is found.
+        for (++ _bufindr; _bufindr < sizeof(cmdbuffer) && cmdbuffer[_bufindr] == 0; ++ _bufindr) ;
+        // If the end of the buffer was empty,
+        if (_bufindr == sizeof(cmdbuffer)) {
+            // skip to the start and find the nonzero command.
+            for (_bufindr = 0; cmdbuffer[_bufindr] == 0; ++ _bufindr) ;
+        }
+    }
+  }
+#endif
+
+#if 0
+  saved_feedrate2 = feedrate; //save feedrate
+#else
+  // Try to deduce the feedrate from the first block of the planner.
+  // Speed is in mm/min.
+  saved_feedrate2 = blocks_queued() ? (block_buffer[block_buffer_tail].nominal_speed * 60.f) : feedrate;
+#endif
+
 	planner_abort_hard(); //abort printing
-	for (int axis = X_AXIS; axis <= E_AXIS; axis++) //save positions
-		saved_pos[axis] = current_position[axis];
-//		saved_pos[axis] = st_get_position_mm(axis);
-	saved_feedrate2 = feedrate; //save feedrate
+	memcpy(saved_pos, current_position, sizeof(saved_pos));
 	saved_active_extruder = active_extruder; //save active_extruder
 
 	saved_extruder_under_pressure = extruder_under_pressure; //extruder under pressure flag - currently unused
@@ -6969,13 +7074,29 @@ void stop_and_save_print_to_ram(float z_move, float e_move)
 //	card.closefile();
 	saved_printing = true;
 	sei();
-	if ((z_move != 0) || (e_move != 0)) // extruder and z move
+	if ((z_move != 0) || (e_move != 0)) { // extruder and z move
+#if 1
+    // Rather than calling plan_buffer_line directly, push the move into the command queue, 
+    char buf[48];
+    strcpy_P(buf, PSTR("G1 Z"));
+    dtostrf(saved_pos[Z_AXIS] + z_move, 8, 3, buf + strlen(buf));
+    strcat_P(buf, PSTR(" E"));
+    // Relative extrusion
+    dtostrf(e_move, 6, 3, buf + strlen(buf));
+    strcat_P(buf, PSTR(" F"));
+    dtostrf(homing_feedrate[Z_AXIS], 8, 3, buf + strlen(buf));
+    // At this point the command queue is empty.
+    enquecommand(buf, false);
+    // If this call is invoked from the main Arduino loop() function, let the caller know that the command
+    // in the command queue is not the original command, but a new one, so it should not be removed from the queue.
+    repeatcommand_front();
+#else
 		plan_buffer_line(saved_pos[X_AXIS], saved_pos[Y_AXIS], saved_pos[Z_AXIS] + z_move, saved_pos[E_AXIS] + e_move, homing_feedrate[Z_AXIS], active_extruder);
     st_synchronize(); //wait moving
-	MYSERIAL.print("SDPOS="); MYSERIAL.println(sdpos_atomic, DEC);
-	MYSERIAL.print("SDLEN_PLAN="); MYSERIAL.println(sdlen_planner, DEC);
-	MYSERIAL.print("SDLEN_CMDQ="); MYSERIAL.println(sdlen_cmdqueue, DEC);
-
+    memcpy(current_position, saved_pos, sizeof(saved_pos));
+    memcpy(destination, current_position, sizeof(destination));
+#endif
+  }
 }
 
 void restore_print_from_ram_and_continue(float e_move)
@@ -6989,7 +7110,10 @@ void restore_print_from_ram_and_continue(float e_move)
 	plan_set_e_position(e);
 	plan_buffer_line(saved_pos[X_AXIS], saved_pos[Y_AXIS], saved_pos[Z_AXIS], saved_pos[E_AXIS], homing_feedrate[Z_AXIS], active_extruder);
     st_synchronize();
+  memcpy(current_position, saved_pos, sizeof(saved_pos));
+  memcpy(destination, current_position, sizeof(destination));
 	card.setIndex(saved_sdpos);
+  sdpos_atomic = saved_sdpos;
 	card.sdprinting = true;
 	saved_printing = false;
 }

Firmware/cmdqueue.cpp

@@ -62,7 +62,6 @@ bool cmdqueue_pop_front()
         } else {
             // There is at least one ready line in the buffer.
             // First skip the current command ID and iterate up to the end of the string.
-//            for (++ bufindr; cmdbuffer[bufindr] != 0; ++ bufindr) ;
             for (bufindr += CMDHDRSIZE; cmdbuffer[bufindr] != 0; ++ bufindr) ;
             // Second, skip the end of string null character and iterate until a nonzero command ID is found.
             for (++ bufindr; bufindr < sizeof(cmdbuffer) && cmdbuffer[bufindr] == 0; ++ bufindr) ;
@@ -92,7 +91,10 @@ bool cmdqueue_pop_front()
 
 void cmdqueue_reset()
 {
-    while (cmdqueue_pop_front()) ;
+    bufindr = 0;
+    bufindw = 0;
+    buflen = 0;
+    cmdbuffer_front_already_processed = false;
 }
 
 // How long a string could be pushed to the front of the command queue?
@@ -552,7 +554,6 @@ void get_command()
   // Reads whole lines from the SD card. Never leaves a half-filled line in the cmdbuffer.
   while( !card.eof() && !stop_buffering) {
     int16_t n=card.get();
-    ++ sd_count.value;
     char serial_char = (char)n;
     if(serial_char == '\n' ||
        serial_char == '\r' ||
@@ -598,6 +599,7 @@ void get_command()
       }
       // The new command buffer could be updated non-atomically, because it is not yet considered
       // to be inside the active queue.
+      sd_count.value = (card.get_sdpos()+1) - sdpos_atomic;
       cmdbuffer[bufindw] = CMDBUFFER_CURRENT_TYPE_SDCARD;
       cmdbuffer[bufindw+1] = sd_count.lohi.lo;
       cmdbuffer[bufindw+2] = sd_count.lohi.hi;
@@ -605,19 +607,20 @@ void get_command()
       // Calculate the length before disabling the interrupts.
       uint8_t len = strlen(cmdbuffer+bufindw+CMDHDRSIZE) + (1 + CMDHDRSIZE);
 
-/*    SERIAL_ECHOPGM("SD cmd(");
-      MYSERIAL.print(sd_count.value, DEC);
-      SERIAL_ECHOPGM(") ");
-      SERIAL_ECHOLN(cmdbuffer+bufindw+CMDHDRSIZE);*/
+//      SERIAL_ECHOPGM("SD cmd(");
+//      MYSERIAL.print(sd_count.value, DEC);
+//      SERIAL_ECHOPGM(") ");
+//      SERIAL_ECHOLN(cmdbuffer+bufindw+CMDHDRSIZE);
 //    SERIAL_ECHOPGM("cmdbuffer:");
 //    MYSERIAL.print(cmdbuffer);
 //    SERIAL_ECHOPGM("buflen:");
 //    MYSERIAL.print(buflen+1);
+      sd_count.value = 0;
 
       cli();
       ++ buflen;
       bufindw += len;
-      sdpos_atomic = card.get_sdpos();
+      sdpos_atomic = card.get_sdpos()+1;
       if (bufindw == sizeof(cmdbuffer))
           bufindw = 0;
       sei();
@@ -640,26 +643,33 @@ void get_command()
 
 uint16_t cmdqueue_calc_sd_length()
 {
-    int _buflen = buflen;
-    int _bufindr = bufindr;
+    if (buflen == 0)
+        return 0;
     union {
         struct {
             char lo;
             char hi;
         } lohi;
         uint16_t value;
-    } sdlen;
-    sdlen.value = 0;
-    while (_buflen--)
-    {
+    } sdlen_single;
+    uint16_t sdlen = 0;
+    for (int _buflen = buflen, _bufindr = bufindr;;) {
         if (cmdbuffer[_bufindr] == CMDBUFFER_CURRENT_TYPE_SDCARD) {
-            sdlen.lohi.lo += cmdbuffer[_bufindr + 1];
-            sdlen.lohi.hi += cmdbuffer[_bufindr + 2];
+            sdlen_single.lohi.lo = cmdbuffer[_bufindr + 1];
+            sdlen_single.lohi.hi = cmdbuffer[_bufindr + 2];
+            sdlen += sdlen_single.value;
         }
-        //skip header, skip command
+        if (-- _buflen == 0)
+            break;
+        // First skip the current command ID and iterate up to the end of the string.
         for (_bufindr += CMDHDRSIZE; cmdbuffer[_bufindr] != 0; ++ _bufindr) ;
-        //skip zeros
+        // Second, skip the end of string null character and iterate until a nonzero command ID is found.
         for (++ _bufindr; _bufindr < sizeof(cmdbuffer) && cmdbuffer[_bufindr] == 0; ++ _bufindr) ;
+        // If the end of the buffer was empty,
+        if (_bufindr == sizeof(cmdbuffer)) {
+            // skip to the start and find the nonzero command.
+            for (_bufindr = 0; cmdbuffer[_bufindr] == 0; ++ _bufindr) ;
+        }
     }
-    return sdlen.value;
-}
+    return sdlen;
+}

Firmware/planner.cpp

@@ -552,10 +552,13 @@ static inline void planner_update_queue_min_counter()
 }
 #endif /* PLANNER_DIAGNOSTICS */
 
+extern volatile uint32_t step_events_completed; // The number of step events executed in the current block
+
 void planner_abort_hard()
 {
     // Abort the stepper routine and flush the planner queue.
-    quickStop();
+    // DISABLE_STEPPER_DRIVER_INTERRUPT
+    TIMSK1 &= ~(1<<OCIE1A);
 
     // Now the front-end (the Marlin_main.cpp with its current_position) is out of sync.
     // First update the planner's current position in the physical motor steps.
@@ -571,9 +574,32 @@ void planner_abort_hard()
     current_position[E_AXIS] = st_get_position_mm(E_AXIS);
     // Apply the mesh bed leveling correction to the Z axis.
 #ifdef MESH_BED_LEVELING
-    if (mbl.active)
-        current_position[Z_AXIS] -= mbl.get_z(current_position[X_AXIS], current_position[Y_AXIS]);
+    if (mbl.active) {
+        if (current_block == NULL || (current_block->steps_x == 0 && current_block->steps_y == 0))
+            current_position[Z_AXIS] -= mbl.get_z(current_position[X_AXIS], current_position[Y_AXIS]);
+        else {
+            float t = float(step_events_completed) / float(current_block->step_event_count);
+            float vec[3] = { 
+              current_block->steps_x / axis_steps_per_unit[X_AXIS],
+              current_block->steps_y / axis_steps_per_unit[Y_AXIS],
+              current_block->steps_z / axis_steps_per_unit[Z_AXIS]
+            };
+            float pos1[3], pos2[3];
+            for (int8_t i = 0; i < 3; ++ i) {
+              if (current_block->direction_bits & (1<<i))
+                vec[i] = - vec[i];
+              pos1[i] = current_position[i] - vec[i] * t;
+              pos2[i] = current_position[i] + vec[i] * (1.f - t);
+            }
+            pos1[Z_AXIS] -= mbl.get_z(pos1[X_AXIS], pos1[Y_AXIS]);
+            pos2[Z_AXIS] -= mbl.get_z(pos2[X_AXIS], pos2[Y_AXIS]);
+            current_position[Z_AXIS] = pos1[Z_AXIS] * t + pos2[Z_AXIS] * (1.f - t);
+        }
+    }
 #endif
+    // Clear the planner queue.
+    quickStop();
+
     // Apply inverse world correction matrix.
     machine2world(current_position[X_AXIS], current_position[Y_AXIS]);
     memcpy(destination, current_position, sizeof(destination));
@@ -1305,8 +1331,7 @@ void planner_add_sd_length(uint16_t sdlen)
   if (block_buffer_head != block_buffer_tail) {
     // The planner buffer is not empty. Get the index of the last buffer line entered,
     // which is (block_buffer_head - 1) modulo BLOCK_BUFFER_SIZE.
-    unsigned char last = (block_buffer_head + BLOCK_BUFFER_SIZE - 1) & (BLOCK_BUFFER_SIZE - 1);
-    block_buffer[last].sdlen += sdlen;
+    block_buffer[prev_block_index(block_buffer_head)].sdlen += sdlen;
   } else {
     // There is no line stored in the planner buffer, which means the last command does not need to be revertible,
     // at a power panic, so the length of this command may be forgotten.

Firmware/stepper.cpp

@@ -61,7 +61,7 @@ static int32_t counter_x,       // Counter variables for the bresenham line trac
                counter_y,
                counter_z,
                counter_e;
-volatile static uint32_t step_events_completed; // The number of step events executed in the current block
+volatile uint32_t step_events_completed; // The number of step events executed in the current block
 static int32_t  acceleration_time, deceleration_time;
 //static unsigned long accelerate_until, decelerate_after, acceleration_rate, initial_rate, final_rate, nominal_rate;
 static uint16_t acc_step_rate; // needed for deccelaration start point