3DPrinting/Prusa-Firmware
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
bubnikv 2017-09-22 13:44:10 +02:00
parent 582a6270b0
commit 72ab17f585
4 changed files with 207 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

158
Firmware/Marlin_main.cpp
View File

@ -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];
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();
if (sdlen)
planner_add_sd_length(sdlen);
planner_add_sd_length(sdlen.value);
sei();
}
cmdbuffer_front_already_processed = false;
}
}
//check heater every n milliseconds
@ -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;
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);
#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
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;
}

48
Firmware/cmdqueue.cpp
View File

@ -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;
}

33
Firmware/planner.cpp
View File

@ -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)
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.

2
Firmware/stepper.cpp
View File

@ -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